Intro

Building working, successful, and long-lasting code is hard. Moving requirements and new features make every year a surgical process, carefully transplanting what code warks and cutting off what doesn't. Fortunately for FTC programmers, libraries are here to provide some stability and a more reasonable starting point.

This tutorial teaches two libraries together: The Qualcomm framework that all FTC teams are required to use, and the ElectronVolts library written by Team 7393, a step above the Qualcomm framework. However, this tutorial leans heavily into the ElectronVolts library. This book is not meant to teach how to do text-based programming, so if you don't know any, you should go study up on Java. Alternatively, if you've already know a curly-brace/c-like language, then it might work out for you to just wing it. This also isn't meant to be a primer on how to use the robot apps; that can be found on the FTC github wiki.

The ElectronVolts Library

What it is:

• A tool for writing FTC code faster
• Classes for reducing boilerplate code
• What this book is trying to get you to use

What it isn't:

• Magic
• Required for FTC participation

The ElectronVolts library is some code on top of the FTC framework, assisting you with writing code legibly. However, learning yet another framework is a time sink, and if the FTC library suits your purposes, it's what you should stick with. Evlib contains helpful stuff, but it only helps if used correctly. You should weigh the costs and benefits of including this new framework, and weigh these problems:

• Value fudging
• Untraceable logic
• Insanity

Library Structure

The library is split up into two parts: electronvolts and evlib. The electronvolts code consists of helper classes which support evlib, is the part of the library you will be using most often.

In the following tutorials, you'll see the LinearOpMode. There are many other items that Qualcomm provides, and can be used along with the ElectronVolts library, but the ElectronVolts library contains its own, preferred Operation Modes, which you will see in the A Starter Program.

Note: Whenever *nix is used here, it refers to operating systems which are somehow based off Unix, like Linux, BSD, and such (excluding Apple operating systems).

Development Environment

Java

This step can usually be ignored, since almost every computer has java preinstalled.

To start, you must first have Java installed. Java 8 or 11 should work, and if you can get Java 16, even better. Windows or MacOS users should go to the Oracle site and follow the installation instructions for your OS. *nix users should follow the distribution-dependent instructions for installing java.

Android Studio

Android Studio is an "IDE", or Integrated Development Environment. It serves the same purpose as a text editor, but has "Integrations", such as code formatting or version control. The most important integration for FTC is donwloading programs onto Android devices.

To install Android Studio for Windows or MacOS computers, go to the Android Studio site and follow the installation instructions for your OS. *nix users should follow the distribution-dependent instructions for installing Android Studio.

You should definitely spend some time customizing and familiarizing yourself with Android Studio.

Project Setup

FTC Framework

Your project is based off the Qualcomm framework. To begin a new FTC project, copy all of the code from the FTC repository into your own project. You can do this by clicking the "Code" button, then clicking "Download ZIP", as shown.

It is better, though, to use Version Control to do this. If you are familiar with github and have a github account, you can do this by "forking" the FTC repository and using that instead.

You can then start writing your code in the TeamCode/src/main/java directory. In Android Studio, you can open this directory by switching from Project to Android view as shown, then navigating to TeamCode/java.

Evlib

The latest version of Evlib is not available in its own repository. You can find a copy of it on the ElectronVolts github page, on the evlib-release branch. Copy all the files in the ./Evlib directory to the same location in your project, then update the settings.gradle file like so:

  include ':FtcRobotController'
  include ':TeamCode'
+ include ':Evlib'

We are currently working on an alternative method to install evlib using the same method as before. Check the original page for updates.

How the robot works

This part is review for those who've already done FTC.

The REV robotics core communicates over wifi with a phone, which controls the robot core. The robot core, in turn, can control connected hardware. You can read more on the FTC docs page.

FTC Framework

The FTC framework expects your code to be written in a certain way so that it can use it in its own classes. In particular, you need to have a class which is marked as "runnable" (either as autonomous or teleop) and is usable (inherits from the com.qualcomm.robotcore.eventloop.opmode.OpMode class).

For example, a wrong way to write FTC code:

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.Servo;

@Autonomous(name = "FooOp")
public class FooOp {
    public static void main(String[] args) {
        Servo s = (Servo) hardwareMap.get("myServo");
        int i = 0;

        while (true) {
            s.setPosition(i);
            i = (i + 10) % 180;
        }
    }
}

There are a few things wrong with this.

• The hardwareMap does not even exist, so you can't control any hardware.
• The main method here will not be run at all -- Qualcomm has their own main code, so yours will not be run.
• The @Autonomous annotation is wrong -- this class is not an "Operation Mode", which is what the framework is expecting. This will probably result in some strange runtime error (TODO: Add a picture of that runtime error).

Here is a correct implementation:

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.Servo;

@Autonomous(name = "FooOp")
public class FooOp extends LinearOpMode {
    @Override
    public void runOpMode() {
        Servo s = (Servo) hardwareMap.get("myServo");
        int i = 0;

        waitForStart();

        while (true) {
            s.setPosition(i);
            i = (i + 10) % 180;
        }
    }
}

All the mentioned problems in the previous code have been corrected here:

• The hardwareMap now exists: It exists in the superclasses of FooOp.
• The @Autonomous annotation now makes sense: It is annotating a class which is a Linear Operation Mode. The Qualcomm framework can now correctly detect and run it.

A Starter Program

To start an ElectronVolts TeleOp project, make a new copy of the Qualcomm framework and add this into your TeamCode directory:

 java
 ├── MyRobotCfg.java
 └── MyTeleOp.java

Then add the ElectronVolts library into the directory like so:

 java
 ├── MyRobotCfg.java
 ├── MyTeleOp.java
 └── ftc
     ├── electronvolts
     │   └┄┄
     ├── evlib
     ┆   └┄┄

Once you've set up those files, you can start writing FTC code.

The end product should look something like below. This is just for later reference (i.e. when your code stops working); keep reading for the rest of the walkthrough.

MyRobotCfg.java

import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import ftc.evlib.hardware.config.RobotCfg;

public class MyRobotCfg extends RobotCfg {
    private final DcMotor myMotor;

    public DcMotor getMyMotor() {
        return myMotor;
    }

    public MyRobotCfg(HardwareMap hardwareMap) {
        super(hardwareMap);
        myMotor = hardwareMap.get(DcMotorEx.class, "bruh");
    }

    @Override
    public void start() {}

    @Override
    public void act() {}

    @Override
    public void stop() {}
}

MyTeleOp.java

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import ftc.electronvolts.util.Function;
import ftc.electronvolts.util.Functions;
import ftc.electronvolts.util.files.Logger;
import ftc.evlib.opmodes.AbstractTeleOp;

@TeleOp(name = "My TeleOp")
public class MyTeleOp extends AbstractTeleOp<MyRobotCfg> {
    DcMotor myMotor;

    @Override
    protected MyRobotCfg createRobotCfg() {
        return new MyRobotCfg(hardwareMap);
    }

    @Override
    protected Logger createLogger() {
        return null;
    }

    @Override
    protected void setup() {
        myMotor = robotCfg.getMyMotor();
    }

    @Override
    protected void setup_act() {}

    @Override
    protected void go() {}

    @Override
    protected void act() {
        double power = gamepad1.left_trigger - gamepad1.right_trigger;
        myMotor.setPower(power);
    }

    @Override
    protected void end() {}

    @Override
    protected Function getJoystickScalingFunction() {
        return Functions.linear(1.0);
    }
}

Robot Config

In order to access hardware, the Qualcomm framework provides an object called the hardwareMap, which is something like a TypeMap, except each type also has its own map between String and the hardware you want.

ElectronVolts takes this a step further by making the abstract class RobotCfg, which can be used to preload all of the names out of the hardwareMap and let you get it directly.

(a side note: I call it hardware here, but it's really a reference to the hardware. As in, an object which you can use to control the hardware.)

Importantly, Robot Configs decouple the hardware team from the software team. When the hardware team connects a device to the Robot's brain, that change needs to be reflected in the code. If the hardware specification and the software logic are in the same place, it makes it hard for both teams to read and write. With the Robot Config, the hardware team can declare in one place what hardware they are using in their robot, and the software team only has to read from one place to figure out what hardware is being used.

This also means that this section is great for both hardware and software team to understand.

The library is still not magic.

If you write code that isn't readable, then putting it all in one place won't necessarily make it better.

Code

Start with a Robot Config like so:

MyRobotCfg.java

import com.qualcomm.robotcore.hardware.HardwareMap;
import ftc.evlib.hardware.config.RobotCfg;

public class MyRobotCfg extends RobotCfg {
    public MyRobotCfg(HardwareMap hardwareMap) {
        super(hardwareMap);
    }

    @Override
    public void start() {}

    @Override
    public void act() {}

    @Override
    public void stop() {}
}

Right now, this config represents an empty bot. It has one brain, one phone, and that's it. This is already pretty powerful – without any custom logic, you have direct access to all the phone's sensors. This is sufficient for an abstract example, but what if we want to add motors to a bot?

Let's start by adding a simple DC motor to a bot, which on the robot core has been programmed to be called "bruh":

public class MyRobotCfg extends RobotCfg {
    public final DcMotor myMotor;

    public MyRobotCfg(HardwareMap hardwareMap) {
        super(hardwareMap);
        myMotor = hardwareMap.get(DcMotorEx.class, "bruh");
    }
}

Some code is not shown for conciseness.

This also works with the getter pattern, like so:

public class MyRobotCfg extends RobotCfg {
    private final DcMotor myMotor;

    public DcMotor getMyMotor() {
        return myMotor;
    }

    public MyRobotCfg(HardwareMap hardwareMap) {
        super(hardwareMap);
        myMotor = hardwareMap.get(DcMotorEx.class, "bruh");
    }
}

Now, when the software team needs the motor, and has a MyRobotCfg, they can call myRobotCfg.getMyMotor() to receive and move the motor.

The complete code should look like this:

import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import ftc.evlib.hardware.config.RobotCfg;

public class MyRobotCfg extends RobotCfg {
    private final DcMotor myMotor;

    public DcMotor getMyMotor() {
        return myMotor;
    }

    public MyRobotCfg(HardwareMap hardwareMap) {
        super(hardwareMap);
        myMotor = hardwareMap.get(DcMotorEx.class, "bruh");
    }

    @Override
    public void start() {}

    @Override
    public void act() {}

    @Override
    public void stop() {}
}

This is all the code you need to move to the next step. If there are some other types of hardware you'd like to add, find them in the FTC javadoc, or see some more types we've made in the Hardware section.

TeleOp Program

A TeleOp is Tele-Operation: A period of time when you, the human, control the robot. A TeleOp therefore needs take input from a source (in 2021, an xbox controller), and somehow transform it into actions done by the robot. That's the part you make.

Let's get right into it, with the following starter code:

MyTeleOp.java

@TeleOp(name = "My TeleOp")
public class MyTeleOp extends AbstractTeleOp<MyRobotCfg> {
    @Override
    protected MyRobotCfg createRobotCfg() {
        return new MyRobotCfg();
    }

    @Override
    protected Logger createLogger() {
        return null;
    }

    @Override
    protected void setup() {}

    @Override
    protected void setup_act() {}

    @Override
    protected void go() {}

    @Override
    protected void act() {}

    @Override
    protected void end() {}

    @Override
    protected Function getJoystickScalingFunction() {
        return Functions.linear(1.0);
    }
}

This is a lot of methods, and we'll break it down more in the chapter 3. For now, we'll set the goal of moving the motor counterclockwise with the left trigger of the controller, and clockwise with the right trigger.

To start, we need to figure out how much the motor should move. To do this, we'll need to transform the input from the controller into an output for the motor to be able to use.

protected void act() {
    double power = gamepad1.left_trigger - gamepad1.right_trigger;
}

You can see that gamepad1.left_trigger is not a reference to hardware, but a value. This is another one of the ElectronVolts library's simplifications: Managing the input values. Next, we want to send this computed value to the DC motor.

protected void act() {
    double power = gamepad1.left_trigger - gamepad1.right_trigger;
    robotCfg.getMyMotor().setPower(power);
}

This is relatively straightforward. Now, after the program is initialized and the play button is pressed, the act function will run in a loop, so you don't have to worry about the power only being set once.

The completed code should look like this:

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;

import ftc.electronvolts.util.Function;
import ftc.electronvolts.util.Functions;
import ftc.electronvolts.util.files.Logger;
import ftc.evlib.opmodes.AbstractTeleOp;

@TeleOp(name = "My TeleOp")
public class MyTeleOp extends AbstractTeleOp<MyRobotCfg> {

    DcMotor myMotor;

    @Override
    protected MyRobotCfg createRobotCfg() {
        return new MyRobotCfg(hardwareMap);
    }

    @Override
    protected Logger createLogger() {
        return null;
    }

    @Override
    protected void setup() {}

    @Override
    protected void setup_act() {}

    @Override
    protected void go() {}

    @Override
    protected void act() {
        double power = gamepad1.left_trigger - gamepad1.right_trigger;
        robotCfg.getMyMotor().setPower(power);
    }

    @Override
    protected void end() {}

    @Override
    protected Function getJoystickScalingFunction() {
        return Functions.linear(1.0);
    }
}

Compile and Install

When using Android Studio normally, the box highlighted here will look like this. The hammer icon on the left is the "build" button - it compiles the code you've written using a tool called Gradle. The play button on the right triggers a "deploy" - it compiles your code, but also copies the compiled code into the robot, if it's connected by USB.

To download your code, you must either have a USB or WiFi connection to the Control Hub. If you use a WiFi connection, it is recommended to use an external wifi card or adapter to be able to connect to both the internet and robot at the same time.

Operation Modes

Operation Modes are the entry point for your code. Each of them contain "runtime stages", which are called at a certain point during the OpMode's lifetime.

For example, as soon as you press the init button on the driver station, the setup method (which is in almost all of our opmodes) is run. If you want to run code during an opmode, you should do it in one of those stages.

Base Operation Mode

The class ftc.evlib.opmodes.AbstractOp represents the most basic conventional OpMode. It allows you to make an opmode that is linked to a Robot Configuration. This class can be used directly by extending it, but it is recommended that you extend AbstractTeleOp or AbstractAutoOp instead to take advantage of their features as well as these.

AbstractOp takes care of the following:

• Timing the match
• Logging values to a file
• Updating the servos
• Giving global access to the telemetry
• Storing the robotCfg

Runtime stages

In order to use the AbstractOp, you must extend and implement the following methods, representing stages that the robot goes through during its operation. The OpMode moves through seven runtime stages, three of which are utility stages. Here they are in order:

1. setup: This method is run once when the init button is pressed.

2. setup_act: After setup has run, this method is run in a loop, which exits when the play button is pressed. This method is guaranteed to not stop before it completes.

3. go: This method is run once at the time when the play button is pressed.

4. pre_act: This method is always run before every time act runs.

5. act: After go has run, this method is run in a loop until the OpMode is stopped by the Driver Station or by the game time.

6. post_act: This method is always run after every time act runs.

7. end: This method is run once after the OpMode is stopped by the Driver Station or by the game timer.

You usually don't deal with all of these directly, since some such as pre_act and post_act are used for supporting more specific OpModes. Others, such as setup and act, are usually meant for you to implement.

You can find the definition of this class at ftc/evlib/opmodes/AbstractOp.java

AbstractTeleOp extends AbstractOp and adds code to handle the gamepads. You can use this to make a TeleOp by extending this class. Here is and example. Inside your class you have access to driver1 and driver2, which are instances of the GamepadManager class. They add edge detection and scaling to the gamepads. See also: AbstractAutoOp

ftc/evlib/opmodes/AbstractTeleOp.java

package ftc.evlib.opmodes;

import ftc.electronvolts.util.Function;
import ftc.electronvolts.util.Functions;
import ftc.electronvolts.util.units.Time;
import ftc.evlib.driverstation.GamepadManager;
import ftc.evlib.hardware.config.RobotCfg;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 9/12/16
 * 
 * extends AbstractOp and adds gamepad edge detection and scaling, and a 2 minute timer
 *
 * @see AbstractOp
 * @see GamepadManager
 */
public abstract class AbstractTeleOp<Type extends RobotCfg> extends AbstractOp<Type> {
    public GamepadManager driver1;
    public GamepadManager driver2;

    /**
     * This is implemented by the teleop opmode
     *
     * @return a Function to scale the joysticks by
     */
    protected abstract Function getJoystickScalingFunction();

    @Override
    public Time getMatchTime() {
        return Time.fromMinutes(2); //teleop is 2 minutes
    }

    @Override
    public void start() {
        //set the joystick deadzone
//        gamepad1.setJoystickDeadzone(.1F);
//        gamepad2.setJoystickDeadzone(.1F);

        //get the scaling function
        Function f = getJoystickScalingFunction();
        if (f == null) {
            //if it is null set it to none
            f = Functions.none();
        }

        //apply the function to the gamepads and store them
        driver1 = new GamepadManager(gamepad1, f);
        driver2 = new GamepadManager(gamepad2, f);
        super.start();
    }

    @Override
    public void pre_act() {
        //update the joystick values
        driver1.update();
        driver2.update();
    }

    @Override
    public void post_act() {

    }
}

AbstractAutoOp extends AbstractOp and adds code to run a state machine. You can create an autonomous by extending this class and creating the StateMachine in the buildStates() method. Here is an example. See also: AbstractTeleOp

ftc/evlib/opmodes/AbstractAutoOp.java

package ftc.evlib.opmodes;

import ftc.electronvolts.statemachine.StateMachine;
import ftc.electronvolts.util.units.Time;
import ftc.evlib.hardware.config.RobotCfg;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 9/13/16
 *
 * extends AbstractOp and adds a 30 second timer and a state machine
 *
 * @see AbstractOp
 * @see StateMachine
 */
public abstract class AbstractAutoOp<Type extends RobotCfg> extends AbstractOp<Type> {
    protected StateMachine stateMachine;

    /**
     * This is implemented by the autonomous opmode
     * It is called where the setup would have been
     *
     * @return A state machine to be run
     */
    public abstract StateMachine buildStates();

    @Override
    public Time getMatchTime() {
        return Time.fromSeconds(30); //autonomous is 30 seconds
    }

    @Override
    public void setup() {
        stateMachine = buildStates(); //get the state machine from the opmode
    }

    @Override
    public void pre_act() {

    }

    @Override
    public void post_act() {
        stateMachine.act(); //update the state machine
    }
}

Go to Servo Presets to see how to extend this class to tune the servos.

Here is AbstractServoTuneOp, which can be extended to tune servos for any Robot Configuration.

ftc/evlib/opmodes/AbstractServoTuneOp.java

package ftc.evlib.opmodes;

import java.util.ArrayList;
import java.util.List;

import ftc.electronvolts.util.Function;
import ftc.electronvolts.util.Functions;
import ftc.electronvolts.util.Utility;
import ftc.electronvolts.util.files.Logger;
import ftc.electronvolts.util.files.OptionsFile;
import ftc.electronvolts.util.units.Time;
import ftc.evlib.hardware.config.RobotCfg;
import ftc.evlib.hardware.servos.ServoCfg;
import ftc.evlib.hardware.servos.ServoControl;
import ftc.evlib.hardware.servos.ServoName;
import ftc.evlib.util.EVConverters;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 9/30/16
 *
 * extends AbstractTeleOp and adds tuning of servo presets with the joysticks.
 *
 * Subclasses of this are very simple since this does most of the work.
 *
 * It allows you to change your servo presets without changing the code and re-deploying it to the
 * phone. This means that you can swap out a servo and re-tune it without having to go into the
 * program and fix magic numbers. Note:  It only works if you use presets everywhere instead of
 * hardcoded values.
 *
 * How to use for your robot:
 * Create a subclass of this (AbstractServoTuneOp).
 * return a new instance of your RobotCfg (it has the servos) in createRobotCfg().
 *
 * Subclass example:
 *
 * <code>
 *
 * \@TeleOp(name = "MyRobot ServoTuneOp")
 * public class MyRobotServoTuneOp extends AbstractServoTuneOp {
 * \@Override protected RobotCfg createRobotCfg() {
 * return new MyRobotCfg(hardwareMap);
 * }
 * }
 * </code>
 *
 * How to operate:
 * Use the dpad up and down to cycle through all the servos
 * Use the dpad left and right to move through the presets for that servo.
 * Use the left and right joystick y values to change the servo position.
 * Press start to save the current preset of the current servo to the current value.
 *
 * The presets are saved in files that are retrieved when you run other opmodes to find the value of each preset.
 *
 * @see ServoControl
 * @see ServoCfg
 */
public abstract class AbstractServoTuneOp extends AbstractTeleOp<RobotCfg> {
    /**
     * The index of the servo in the list
     */
    private int servoIndex = 0;

    /**
     * The index of the preset for the current servo
     */
    private int presetIndex = 0;

    /**
     * records whether or not a new servo has been selected
     */
    private boolean servoIndexChanged = true;

    /**
     * records whether or not a new servo preset has been selected
     */
    private boolean servoPresetIndexChanged = true;

    /**
     * The list of current positions for each servo
     */
    private final List<Double> servoPositions = new ArrayList<>();

    /**
     * The list of servo names
     */
    private List<ServoName> servoNames;

    /**
     * The list of preset names for the current servo
     */
    private List<Enum> presetNames;

    /**
     * The list of preset values for the current servo
     */
    private List<Double> presetValues;

    /**
     * The current servo
     */
    private ServoControl servo;

    /**
     * @return no joystick scaling
     */
    @Override
    protected Function getJoystickScalingFunction() {
        return Functions.none();
    }

    /**
     * @return no match timer
     */
    @Override
    public Time getMatchTime() {
        return null;
    }

    /**
     * @return no logging
     */
    @Override
    protected Logger createLogger() {
        return null;
    }

    @Override
    protected void setup() {
        //get a list of servo names from the RobotCfg
        servoNames = robotCfg.getServos().getServoNames();

        //add servo positions to be the same length as servoNames
        for (ServoName ignored : servoNames) {
            servoPositions.add(0.5);
        }
    }

    @Override
    protected void setup_act() {

    }

    @Override
    protected void go() {

    }

    @Override
    protected void act() {


        //if dpad up is pressed
        if (driver1.dpad_up.justPressed() || driver2.dpad_up.justPressed()) {
            servoIndex += 1; //move to the next servo
            //wrap around if the index is too large
            if (servoIndex > servoNames.size() - 1) servoIndex = 0;
            servoIndexChanged = true; //signal that the index changed
        }

        //if dpad down is pressed
        if (driver1.dpad_down.justPressed() || driver2.dpad_down.justPressed()) {
            servoIndex -= 1; //move to the previous servo
            //wrap around if the index is too small
            if (servoIndex < 0) servoIndex = servoNames.size() - 1;
            servoIndexChanged = true; //signal that the index changed
        }

        //if a different servo was selected
        if (servoIndexChanged) {
            servoIndexChanged = false;

            servo = robotCfg.getServo(servoNames.get(servoIndex));//get the servo
            presetNames = new ArrayList<>(servo.getPresets().keySet()); //get the preset names from the servo
            presetValues = new ArrayList<>(servo.getPresets().values()); //get the presets from the servo

            presetIndex = 0; //start at the first preset for the new servo
            servoPresetIndexChanged = true; //signal to reload the servo preset
        }

        //get the servo position
        double servoPosition = servoPositions.get(servoIndex);

        //if the dpad left was just pressed
        if (driver1.dpad_left.justPressed() || driver2.dpad_left.justPressed()) {
            presetIndex -= 1; //select the previous servo preset
            //wrap around if the index is too small
            if (presetIndex < 0) presetIndex = presetValues.size() - 1;
            servoPresetIndexChanged = true; //signal that the index changed
        }

        //if the dpad right was just pressed
        if (driver1.dpad_right.justPressed() || driver2.dpad_right.justPressed()) {
            presetIndex += 1; //select the next servo preset
            //wrap around if the index is too large
            if (presetIndex > presetValues.size() - 1) presetIndex = 0;
            servoPresetIndexChanged = true; //signal that the index changed
        }

        //is the servo preset index changed
        if (servoPresetIndexChanged) {
            servoPresetIndexChanged = false;
            servoPosition = presetValues.get(presetIndex); //set the servo to the preset position
        }

        telemetry.addData("Press start to set the current preset to the current value", "");
        //if start is pressed, save the current preset to a file
        if (driver1.start.justPressed() || driver2.start.justPressed()) {
            //set the current selected preset to the current servo position
            servo.getPresets().put(presetNames.get(presetIndex), servoPosition);
            presetValues.set(presetIndex, servoPosition);

            OptionsFile optionsFile = new OptionsFile(EVConverters.getInstance()); //create an OptionsFile

            //put the preset names and presets into the OptionsFile
            for (int i = 0; i < presetNames.size(); i++) {
                optionsFile.set(presetNames.get(i).name(), presetValues.get(i).toString());
            }

            optionsFile.writeToFile(ServoCfg.getServoFile(servoNames.get(servoIndex))); //store the OptionsFile to a file
        }

        //modify the servo position using the joysticks
        servoPosition += 2e-4 * matchTimer.getDeltaTime() * (driver1.left_stick_y.getValue() + 0.1 * driver1.right_stick_y.getValue() + driver2.left_stick_y.getValue() + 0.1 * driver2.right_stick_y.getValue());

        //limit the position
        servoPosition = Utility.servoLimit(servoPosition);

        //set the servo to the position
        servo.setPosition(servoPosition);

        //store the position
        servoPositions.set(servoIndex, servoPosition);

        //display telemetry about the servo
        telemetry.addData("Servo Name", servoNames.get(servoIndex));
        telemetry.addData("Servo Preset Name", presetNames.get(presetIndex));
        telemetry.addData("Servo Preset Value", servoPosition);
    }

    @Override
    protected void end() {

    }
}

This page is out of date. You can find a new guide to options here

AbstractOptionsOp extends AbstractOp. You can use it to take input from the gamepad and store settings for autonomous in a file on the robot phone. It uses OptionsFile from the state-machine-framework.

ftc/evlib/opmodes/AbstractOptionsOp.java

package ftc.evlib.opmodes;

import ftc.electronvolts.util.files.Logger;
import ftc.electronvolts.util.files.OptionsFile;
import ftc.electronvolts.util.units.Time;
import ftc.evlib.hardware.config.RobotCfg;
import ftc.evlib.util.EVConverters;
import ftc.evlib.util.FileUtil;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/29/16
 *
 * extends AbstractTeleOp and adds saving and loading an OptionsFile and removes the match timer
 *
 * @see AbstractTeleOp
 * @see OptionsFile
 */
public abstract class AbstractOptionsOp extends AbstractTeleOp<RobotCfg> {
    private final String filename;
    public OptionsFile optionsFile;

    /**
     * The filename will be set by the subclasses
     *
     * @param filename the name of the file where the options are stored
     */
    public AbstractOptionsOp(String filename) {
        this.filename = filename;
    }

    /**
     * @return a dummy RobotCfg
     */
    @Override
    protected RobotCfg createRobotCfg() {
        return new RobotCfg(hardwareMap) {
            @Override
            public void act() {

            }

            @Override
            public void stop() {

            }
        };
    }

    @Override
    protected Logger createLogger() {
        //the OptionsOp has no logging
        return null;
    }

    @Override
    public Time getMatchTime() {
        //the OptionsOp has no time limit
        return null;
    }

    /**
     * Load the options from the file
     */
    public void loadOptionsFile() {
        optionsFile = new OptionsFile(EVConverters.getInstance(), FileUtil.getOptionsFile(filename));
    }

    /**
     * save the options from the file
     */
    public void saveOptionsFile() {
        optionsFile.writeToFile(FileUtil.getOptionsFile(filename));
    }

    @Override
    protected void setup() {
        //load the file when the opmode starts
        loadOptionsFile();
    }

    @Override
    protected void setup_act() {

    }

    @Override
    protected void go() {

    }

    @Override
    public void post_act() {
        super.post_act();

        //display telemetry instructions
        telemetry.addData("* back button => erase changes", "");
        //reload the file if the back button is pressed
        if (driver1.back.justPressed()) loadOptionsFile();

        //display telemetry instructions
        telemetry.addData("* start button => save", "");
        //save the file if the start button is pressed
        if (driver1.start.justPressed()) saveOptionsFile();

        telemetry.addData("* Stop the opmode to save and quit.", "");
    }

    @Override
    protected void end() {
        //save the file when the opmode ends
        saveOptionsFile();
    }
}

EVStates | EVEndConditions | EVStateMachineBuilder

EVStates is a factory class that extends States from the state-machine-framework. It inherits all the factory methods from States, and adds its own that relate to FTC.

Some that we use frequently are:

• mecanumDrive -- drives with the mecanum wheels and stabilizes with the gyro
• servoTurn -- turns a servo to a preset at a given speed
• calibrateGyro -- waits for the gyro sensor to finish calibrating

Some others that are also useful are:

• drive -- drives with a TwoMotors object
• turn -- tuns with a TwoMotors object
• oneWheelTurn -- powers one wheel to turn
• motorTurn -- turns a motor

This example uses guava to initialize lists, which we have left for alternative methods.

ftc/evlib/statemachine/EVStates.java

package ftc.evlib.statemachine;

import com.google.common.collect.ImmutableList;
import com.qualcomm.robotcore.hardware.GyroSensor;

import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;

import java.util.List;

import ftc.electronvolts.statemachine.AbstractState;
import ftc.electronvolts.statemachine.BasicAbstractState;
import ftc.electronvolts.statemachine.EndCondition;
import ftc.electronvolts.statemachine.EndConditions;
import ftc.electronvolts.statemachine.State;
import ftc.electronvolts.statemachine.StateName;
import ftc.electronvolts.statemachine.States;
import ftc.electronvolts.statemachine.Transition;
import ftc.electronvolts.util.InputExtractor;
import ftc.electronvolts.util.ResultReceiver;
import ftc.electronvolts.util.TeamColor;
import ftc.electronvolts.util.units.Angle;
import ftc.electronvolts.util.units.Distance;
import ftc.electronvolts.util.units.Time;
import ftc.evlib.driverstation.Telem;
import ftc.evlib.hardware.control.LineUpControl;
import ftc.evlib.hardware.control.MecanumControl;
import ftc.evlib.hardware.control.RotationControl;
import ftc.evlib.hardware.control.RotationControls;
import ftc.evlib.hardware.control.TranslationControl;
import ftc.evlib.hardware.control.TranslationControls;
import ftc.evlib.hardware.mechanisms.Shooter;
import ftc.evlib.hardware.motors.MecanumMotors;
import ftc.evlib.hardware.motors.Motor;
import ftc.evlib.hardware.motors.NMotors;
import ftc.evlib.hardware.motors.TwoMotors;
import ftc.evlib.hardware.sensors.DigitalSensor;
import ftc.evlib.hardware.sensors.DistanceSensor;
import ftc.evlib.hardware.sensors.DoubleLineSensor;
import ftc.evlib.hardware.sensors.LineSensorArray;
import ftc.evlib.hardware.servos.ServoControl;
import ftc.evlib.hardware.servos.Servos;
import ftc.evlib.vision.framegrabber.FrameGrabber;
import ftc.evlib.vision.framegrabber.VuforiaFrameFeeder;
import ftc.evlib.vision.processors.BeaconColorResult;
import ftc.evlib.vision.processors.BeaconName;
import ftc.evlib.vision.processors.CloseUpColorProcessor;
import ftc.evlib.vision.processors.ImageProcessor;
import ftc.evlib.vision.processors.ImageProcessorResult;
import ftc.evlib.vision.processors.Location;
import ftc.evlib.vision.processors.RGBBeaconProcessor;
import ftc.evlib.vision.processors.VuforiaBeaconColorProcessor;

import static ftc.evlib.driverstation.Telem.telemetry;
import static ftc.evlib.vision.framegrabber.GlobalFrameGrabber.frameGrabber;
import static ftc.evlib.vision.framegrabber.VuforiaFrameFeeder.beacons;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 5/10/16
 *
 * @see State
 * @see EVStateMachineBuilder
 */
public class EVStates extends States {

    /**
     * Displays the left and right color of a BeaconColorResult
     *
     * @param stateName the name of the state
     * @param receiver  the ResultReceiver to get the color from
     * @return the created State
     * @see BeaconColorResult
     */
    public static State displayBeaconColorResult(StateName stateName, final ResultReceiver<BeaconColorResult> receiver) {
        return new BasicAbstractState(stateName) {
            @Override
            public void init() {

            }

            @Override
            public boolean isDone() {
                Telem.displayBeaconColorResult(receiver);
                return false;
            }

            @Override
            public StateName getNextStateName() {
                return null;
            }
        };
    }

    /**
     * Displays the color of a BeaconColorResult.BeaconColor
     *
     * @param stateName the name of the state
     * @param receiver  the ResultReceiver to get the color from
     * @return the created State
     * @see BeaconColorResult
     */
    public static State displayBeaconColor(StateName stateName, final ResultReceiver<BeaconColorResult.BeaconColor> receiver) {
        return new BasicAbstractState(stateName) {
            @Override
            public void init() {

            }

            @Override
            public boolean isDone() {
                Telem.displayBeaconColor(receiver);
                return false;
            }

            @Override
            public StateName getNextStateName() {
                return null;
            }
        };
    }

    /**
     * Uses vuforia to find the beacon target image, then uses opencv to determine the beacon color
     *
     * @param stateName         the name of the state
     * @param successState      the state to go to if it succeeds
     * @param failState         the state to go to if it fails
     * @param timeoutState      the state to go to if it times out
     * @param timeoutTime       the time before it will time out
     * @param vuforiaReceiver   the ResultReceiver to get the VuforiaFramFeeder object from
     * @param beaconColorResult the ResultReceiver to store the result in
     * @param teamColor         your team's color to decide which beacons to look for
     * @param numFrames         the number of frames to process
     * @param saveImages        whether or not to save the frames for logging
     * @return the created State
     * @see VuforiaFrameFeeder
     * @see VuforiaBeaconColorProcessor
     */
    //TODO assume that vuforia is initialized in findBeaconColorState
    public static State findBeaconColorState(StateName stateName, final StateName successState, final StateName failState, final StateName timeoutState, Time timeoutTime, final ResultReceiver<VuforiaFrameFeeder> vuforiaReceiver, final ResultReceiver<BeaconColorResult> beaconColorResult, TeamColor teamColor, final int numFrames, final boolean saveImages) {
        final List<BeaconName> beaconNames = BeaconName.getNamesForTeamColor(teamColor);
        final EndCondition timeout = EndConditions.timed(timeoutTime);

        return new BasicAbstractState(stateName) {
            private VuforiaFrameFeeder vuforia = null;
            private VuforiaBeaconColorProcessor processor = null;
            private BeaconName beaconName;
            private int beaconIndex = 0; //index of the beaconNames list
            private boolean timedOut = false;

            @Override
            public void init() {
                timeout.init();
                timedOut = false;

                if (beaconIndex >= beaconNames.size()) {
                    beaconIndex = 0;
                    //we should never go here
                }
                beaconName = beaconNames.get(beaconIndex);
                if (processor != null) {
                    processor.setBeaconName(beaconName);
                }
            }

            @Override
            public boolean isDone() {
                if (vuforia == null && vuforiaReceiver.isReady()) {
                    vuforia = vuforiaReceiver.getValue();
//                    if (vuforia == null) {
//                        Log.e("EVStates", "vuforia is null!!!!!!!!!!!!!");
//                    }

                    processor = new VuforiaBeaconColorProcessor(vuforia);
                    processor.setBeaconName(beaconName);

                    VuforiaTrackable beacon = beacons.get(beaconName);
                    beacon.getTrackables().activate();

                    frameGrabber.setImageProcessor(processor);
                    frameGrabber.setSaveImages(saveImages);
                    frameGrabber.grabContinuousFrames();
                }
                timedOut = timeout.isDone();
                return timedOut || processor != null && processor.getResultsFound() >= numFrames;
            }

            @Override
            public StateName getNextStateName() {
                beaconIndex++;
                frameGrabber.stopFrameGrabber();
                VuforiaTrackable beacon = beacons.get(beaconName);
                beacon.getTrackables().deactivate();

                BeaconColorResult result = processor.getAverageResult();
                processor.reset();
                BeaconColorResult.BeaconColor leftColor = result.getLeftColor();
                BeaconColorResult.BeaconColor rightColor = result.getRightColor();
                if ((leftColor == BeaconColorResult.BeaconColor.RED && rightColor == BeaconColorResult.BeaconColor.BLUE)
                        || (leftColor == BeaconColorResult.BeaconColor.BLUE && rightColor == BeaconColorResult.BeaconColor.RED)) {
                    beaconColorResult.setValue(result);
                    return successState;
                } else {
                    beaconColorResult.setValue(new BeaconColorResult());
                    if (timedOut) {
                        return timeoutState;
                    } else {
                        return failState;
                    }
                }
            }
        };
    }

    public static State findColorState(StateName stateName, final StateName successState, final StateName unknownState, final ResultReceiver<VuforiaFrameFeeder> vuforiaReceiver, final ResultReceiver<BeaconColorResult.BeaconColor> colorResult, final boolean saveImages) {
        return new BasicAbstractState(stateName) {
            private VuforiaFrameFeeder vuforia = null;
            private CloseUpColorProcessor processor = null;
            private boolean timedOut = false;

            @Override
            public void init() {
                vuforia = vuforiaReceiver.getValue();
                processor = new CloseUpColorProcessor();

                frameGrabber.setImageProcessor(processor);
                frameGrabber.setSaveImages(saveImages);
                frameGrabber.grabSingleFrame();
            }

            @Override
            public boolean isDone() {
                return frameGrabber.isResultReady();
            }

            @Override
            public StateName getNextStateName() {
                BeaconColorResult.BeaconColor result = (BeaconColorResult.BeaconColor) frameGrabber.getResult().getResult();
                colorResult.setValue(result);
                if (result == BeaconColorResult.BeaconColor.RED || result == BeaconColorResult.BeaconColor.BLUE) {
                    return successState;
                } else {
                    return unknownState;
                }
            }
        };
    }

    public static State beaconColorSwitch(StateName stateName, final StateName redState, final StateName blueState, final StateName unknownState, ResultReceiver<BeaconColorResult.BeaconColor> colorResult) {
        return new BasicAbstractState(stateName) {
            @Override
            public void init() {

            }

            @Override
            public boolean isDone() {
                return true;
            }

            @Override
            public StateName getNextStateName() {
                BeaconColorResult.BeaconColor result = (BeaconColorResult.BeaconColor) frameGrabber.getResult().getResult();
                switch (result) {
                    case RED:
                        return redState;
                    case BLUE:
                        return blueState;
                    default:
                        return unknownState;
                }
            }
        };
    }

    /**
     * use the camera to detect and drive up to the beacon
     *
     * @param stateName       the name of the state
     * @param doneState       the state to go to if it works
     * @param lostObjectState the state to go to if it cannot find the beacon
     * @param timeoutState    the state to go to if it times out
     * @param timeoutMillis   the number of milliseconds before the timeout
     * @param mecanumControl  the mecanum wheels
     * @param frameGrabber    access to the camera frames
     * @return the created State
     */
    public static State mecanumCameraTrack(StateName stateName, final StateName doneState, final StateName lostObjectState, final StateName timeoutState, long timeoutMillis, final MecanumControl mecanumControl, final FrameGrabber frameGrabber, ImageProcessor<? extends Location> imageProcessor) {
        mecanumControl.setDriveMode(MecanumMotors.MecanumDriveMode.NORMALIZED);
        final TranslationControl beaconTrackingControl = TranslationControls.cameraTracking(frameGrabber, imageProcessor);
        final long timeoutTime = System.currentTimeMillis() + timeoutMillis;

        return new BasicAbstractState(stateName) {
            @Override
            public void init() {
                mecanumControl.setTranslationControl(beaconTrackingControl);
            }

            @Override
            public boolean isDone() {
                return !mecanumControl.translationWorked() || beaconTrackingControl.getTranslation().getLength() < 0.1 || System.currentTimeMillis() >= timeoutTime;
            }

            @Override
            public StateName getNextStateName() {
                mecanumControl.stop();

                if (beaconTrackingControl.getTranslation().getLength() < 0.1) {
                    return doneState;
                } else {
                    if (!mecanumControl.translationWorked()) {
                        return lostObjectState;
                    } else {
                        return timeoutState;
                    }
                }
            }
        };
    }

    /**
     * @param stateName      the name of the state
     * @param nextStateName  the name of the next state
     * @param imageProcessor the object that processes the image
     * @param resultReceiver the object that stores the image
     * @return the created State
     */
    public static State processFrame(StateName stateName, final StateName nextStateName, final ImageProcessor imageProcessor, final ResultReceiver<ImageProcessorResult> resultReceiver) {
        return new BasicAbstractState(stateName) {
            @Override
            public void init() {
                frameGrabber.setImageProcessor(imageProcessor);
                frameGrabber.grabSingleFrame();
            }

            @Override
            public boolean isDone() {
                return frameGrabber.isResultReady();
            }

            @Override
            public StateName getNextStateName() {
                resultReceiver.setValue(frameGrabber.getResult());
                return nextStateName;
            }
        };
    }

    /**
     * @param stateName           the name of the state
     * @param unknownUnknownState if both sides are unknown
     * @param unknownRedState     if the left is unknown and the right is red
     * @param unknownBlueState    if the left is unknown and the right is blue
     * @param redUnknownState     if the left is red and the right is unknown
     * @param redRedState         if the left is red and the right is red
     * @param redBlueState        if the left is red and the right is blue
     * @param blueUnknownState    if the left is blue and the right is unknown
     * @param blueRedState        if the left is blue and the right is red
     * @param blueBlueState       if the left is blue and the right is blue
     * @return the created State
     */
    public static State processBeaconPicture(StateName stateName,
                                             final StateName unknownUnknownState, final StateName unknownRedState, final StateName unknownBlueState,
                                             final StateName redUnknownState, final StateName redRedState, final StateName redBlueState,
                                             final StateName blueUnknownState, final StateName blueRedState, final StateName blueBlueState
    ) {

        return new BasicAbstractState(stateName) {
            @Override
            public void init() {
                frameGrabber.setImageProcessor(new RGBBeaconProcessor());
                frameGrabber.grabSingleFrame();
            }

            @Override
            public boolean isDone() {
                return frameGrabber.isResultReady();
            }

            @Override
            public StateName getNextStateName() {
                BeaconColorResult beaconColorResult = (BeaconColorResult) frameGrabber.getResult().getResult();
                BeaconColorResult.BeaconColor leftColor = beaconColorResult.getLeftColor();
                BeaconColorResult.BeaconColor rightColor = beaconColorResult.getRightColor();

                if (leftColor == BeaconColorResult.BeaconColor.RED) {
                    if (rightColor == BeaconColorResult.BeaconColor.RED) {
                        return redRedState;
                    } else if (rightColor == BeaconColorResult.BeaconColor.BLUE) {
                        return redBlueState;
                    } else {
                        return redUnknownState;
                    }
                } else if (leftColor == BeaconColorResult.BeaconColor.BLUE) {
                    if (rightColor == BeaconColorResult.BeaconColor.RED) {
                        return blueRedState;
                    } else if (rightColor == BeaconColorResult.BeaconColor.BLUE) {
                        return blueBlueState;
                    } else {
                        return blueUnknownState;
                    }
                } else {
                    if (rightColor == BeaconColorResult.BeaconColor.RED) {
                        return unknownRedState;
                    } else if (rightColor == BeaconColorResult.BeaconColor.BLUE) {
                        return unknownBlueState;
                    } else {
                        return unknownUnknownState;
                    }
                }
            }
        };
    }

    /**
     * @param stateName the name of the state
     * @param nMotors   the motors to turn off
     * @return the created State
     */
    public static State stop(StateName stateName, final NMotors nMotors) {
        return new BasicAbstractState(stateName) {
            @Override
            public void init() {
                nMotors.stop();
            }

            @Override
            public boolean isDone() {
                return false;
            }

            @Override
            public StateName getNextStateName() {
                return null;
            }
        };
    }

    /**
     * @param stateName      the name of the state
     * @param mecanumControl the motors to turn off
     * @return the created State
     */
    public static State stop(StateName stateName, final MecanumControl mecanumControl) {
        return new BasicAbstractState(stateName) {
            @Override
            public void init() {
                mecanumControl.stop();
            }

            @Override
            public boolean isDone() {
                return false;
            }

            @Override
            public StateName getNextStateName() {
                return null;
            }
        };
    }

    /**
     * @param stateName the name of the state
     * @param message   the message to display to the driver station
     * @param value     the value associated with that message
     * @return the created State
     */
    public static State telemetry(StateName stateName, final String message, final double value) {
        return new BasicAbstractState(stateName) {
            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                telemetry.addData(message, value);
                return false;
            }

            @Override
            public StateName getNextStateName() {
                return null;
            }
        };
    }

    /**
     * @param stateName the name of the state
     * @param message1  the first message to display to the driver station
     * @param input1    an InputExtractor that returns the value associated with the first message
     * @param message2  the second message to display to the driver station
     * @param input2    an InputExtractor that returns the value associated with the second message
     * @return the created State
     */
    public static State telemetry(StateName stateName, final String message1, final InputExtractor<Double> input1, final String message2, final InputExtractor<Double> input2) {
        return new BasicAbstractState(stateName) {
            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                telemetry.addData(message1, input1.getValue());
                telemetry.addData(message2, input2.getValue());
                return false;
            }

            @Override
            public StateName getNextStateName() {
                return null;
            }
        };
    }

    /**
     * @param stateName   the name of the state
     * @param transitions the list of transitions to the next states
     * @param message     the message to display to the driver station
     * @param value       the value associated with that message
     * @return the created State
     */
    public static State telemetry(StateName stateName, List<Transition> transitions, final String message, final double value) {
        return new AbstractState(stateName, transitions) {
            @Override
            public void init() {
            }

            @Override
            public void run() {
                telemetry.addData(message, value);
            }

            @Override
            public void dispose() {
            }
        };
    }

    /**
     * @param stateName   the name of the state
     * @param transitions the list of transitions to the next states
     * @param message1    the first message to display to the driver station
     * @param input1      an InputExtractor that returns the value associated with the first message
     * @param message2    the second message to display to the driver station
     * @param input2      an InputExtractor that returns the value associated with the second message
     * @return the created State
     */
    public static State telemetry(StateName stateName, List<Transition> transitions, final String message1, final InputExtractor<Double> input1, final String message2, final InputExtractor<Double> input2) {
        return new AbstractState(stateName, transitions) {
            @Override
            public void init() {
            }

            @Override
            public void run() {
                telemetry.addData(message1, input1.getValue());
                telemetry.addData(message2, input2.getValue());
            }

            @Override
            public void dispose() {
            }
        };
    }

    /**
     * @param stateName     the name of the state
     * @param nextStateName the name of the next state
     * @param servos        the servos to be initialized
     * @return the created State
     * @see Servos
     */
    public static State servoInit(StateName stateName, final StateName nextStateName, final Servos servos) {
        return new BasicAbstractState(stateName) {
            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                return servos.areServosDone();
            }

            @Override
            public StateName getNextStateName() {
                return nextStateName;
            }
        };
    }

    /**
     * @param stateName     the name of the state
     * @param nextStateName the name of the next state
     * @param gyro          the gyro sensor to be calibrated
     * @return the created State
     * @see GyroSensor
     */
    public static State calibrateGyro(StateName stateName, final StateName nextStateName, final GyroSensor gyro) {
        gyro.calibrate();
        return new BasicAbstractState(stateName) {
            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                return !gyro.isCalibrating();
            }

            @Override
            public StateName getNextStateName() {
                return nextStateName;
            }
        };
    }

    /**
     * @param stateName        the name of the state
     * @param nextStateName    the name of the next state
     * @param doubleLineSensor the 2 line sensors to be calibrated
     * @return the created State
     * @see DoubleLineSensor
     */
    public static State calibrateLineSensor(StateName stateName, final StateName nextStateName, final DoubleLineSensor doubleLineSensor) {
        doubleLineSensor.calibrate();
        return new BasicAbstractState(stateName) {
            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                return doubleLineSensor.isReady();
            }

            @Override
            public StateName getNextStateName() {
                return nextStateName;
            }
        };
    }

    /**
     * Turn a servo to a preset at max speed
     *
     * @param stateName     the name of the state
     * @param nextStateName the name of the state to go to next
     * @param servoControl  the servo
     * @param servoPreset   the preset to go to
     * @param waitForDone   whether to wait for the servo to finish turning or move to the next state immediately
     * @return the created State
     * @see ServoControl
     */
    public static State servoTurn(StateName stateName, StateName nextStateName, ServoControl servoControl, Enum servoPreset, boolean waitForDone) {
        return servoTurn(stateName, nextStateName, servoControl, servoPreset, ServoControl.MAX_SPEED, waitForDone);
    }

    /**
     * Turn a servo to a preset at a given speed
     *
     * @param stateName     the name of the state
     * @param nextStateName the name of the state to go to next
     * @param servoControl  the servo
     * @param servoPreset   the preset to go to
     * @param speed         the speed to turn the servo at
     * @param waitForDone   whether to wait for the servo to finish turning or move to the next state immediately
     * @return the created State
     * @see ServoControl
     */
    public static State servoTurn(StateName stateName, final StateName nextStateName, final ServoControl servoControl, final Enum servoPreset, final double speed, final boolean waitForDone) {
        return new BasicAbstractState(stateName) {

            @Override
            public void init() {
                servoControl.goToPreset(servoPreset, speed);
            }

            @Override
            public boolean isDone() {
                return !waitForDone || servoControl.isDone();
            }

            @Override
            public StateName getNextStateName() {
                return nextStateName;
            }
        };
    }

    /**
     * Turn a servo to a position at max speed
     *
     * @param stateName     the name of the state
     * @param nextStateName the name of the state to go to next
     * @param servoControl  the servo
     * @param servoPosition the position to go to
     * @param waitForDone   whether to wait for the servo to finish turning or move to the next state immediately
     * @return the created State
     * @see ServoControl
     */
    public static State servoTurn(StateName stateName, StateName nextStateName, ServoControl servoControl, double servoPosition, boolean waitForDone) {
        return servoTurn(stateName, nextStateName, servoControl, servoPosition, ServoControl.MAX_SPEED, waitForDone);
    }

    /**
     * Turn a servo to a position at a given speed
     *
     * @param stateName     the name of the state
     * @param nextStateName the name of the state to go to next
     * @param servoControl  the servo
     * @param servoPosition the position to go to
     * @param speed         the speed to turn the servo at
     * @param waitForDone   whether to wait for the servo to finish turning or move to the next state immediately
     * @return the created State
     * @see ServoControl
     */
    public static State servoTurn(StateName stateName, final StateName nextStateName, final ServoControl servoControl, final double servoPosition, final double speed, final boolean waitForDone) {
        return new BasicAbstractState(stateName) {

            @Override
            public void init() {
                servoControl.setPosition(servoPosition, speed);
            }

            @Override
            public boolean isDone() {
                return !waitForDone || servoControl.isDone();
            }

            @Override
            public StateName getNextStateName() {
                return nextStateName;
            }
        };
    }


    /**
     * drive using the mecanum wheels
     * travels for a certain amount of time defined by the robots speed and a desired distance
     *
     * @param stateName       the name of the state
     * @param nextStateName   the next state to go to
     * @param distance        the distance to travel
     * @param mecanumControl  the mecanum wheels
     * @param gyro            the gyro sensor
     * @param velocity        the velocity to drive at
     * @param direction       the direction to drive
     * @param orientation     the angle to rotate to
     * @param maxAngularSpeed the max speed to rotate to that angle
     * @return the created State
     * @see MecanumControl
     * @see GyroSensor
     * @see Distance
     */
    public static State mecanumDrive(StateName stateName, final StateName nextStateName, Distance distance, final MecanumControl mecanumControl, final GyroSensor gyro, final double velocity, final Angle direction, final Angle orientation, final Angle tolerance, final double maxAngularSpeed) {
        mecanumControl.setDriveMode(MecanumMotors.MecanumDriveMode.NORMALIZED);
        double speedMetersPerMillisecond = mecanumControl.getMaxRobotSpeed().metersPerMillisecond() * velocity;
        final double durationMillis = Math.abs(distance.meters() / speedMetersPerMillisecond);
        final EndCondition gyroEC = EVEndConditions.gyroCloseTo(gyro, orientation, tolerance);
        return new BasicAbstractState(stateName) {
            long startTime = 0;

            @Override
            public void init() {
                mecanumControl.setControl(
                        TranslationControls.constant(velocity, direction),
                        RotationControls.gyro(gyro, orientation, maxAngularSpeed)
                );
                startTime = System.currentTimeMillis();
            }

            @Override
            public boolean isDone() {
                long now = System.currentTimeMillis();
                long elapsedTime = now - startTime;
                if (elapsedTime >= durationMillis) {
                    mecanumControl.setTranslationControl(TranslationControls.ZERO);
                    return gyroEC.isDone();
                }
                return false;
            }

            @Override
            public StateName getNextStateName() {
                mecanumControl.stop();
                return nextStateName;
            }
        };
    }

    /**
     * drive using the mecanum wheels
     *
     * @param stateName       the name of the state
     * @param transitions     the list of transitions to the next states
     * @param mecanumControl  the mecanum wheels
     * @param gyro            the gyro sensor
     * @param velocity        the velocity to drive at
     * @param direction       the direction to drive
     * @param orientation     the angle to rotate to
     * @param maxAngularSpeed the max speed to rotate to that angle
     * @return the created State
     * @see MecanumControl
     * @see GyroSensor
     */
    public static State mecanumDrive(StateName stateName, List<Transition> transitions, final MecanumControl mecanumControl, final GyroSensor gyro, final double velocity, final Angle direction, final Angle orientation, final double maxAngularSpeed) {
        mecanumControl.setDriveMode(MecanumMotors.MecanumDriveMode.NORMALIZED);
        return new AbstractState(stateName, transitions) {
            @Override
            public void init() {
//                OptionsFile optionsFile = new OptionsFile(EVConverters.getInstance(), FileUtil.getOptionsFile("AutoOptions.txt"));
//
//                double max = optionsFile.get("gyro_max", Double.class);
//                double gain = optionsFile.get("gyro_gain", Double.class);

                mecanumControl.setControl(
                        TranslationControls.constant(velocity, direction),
                        RotationControls.gyro(gyro, orientation, maxAngularSpeed)
//                        RotationControls.gyro(gyro, orientation, max, false, gain)
                );
            }

            @Override
            public void run() {

            }

            @Override
            public void dispose() {
                mecanumControl.stop();
            }
        };
    }

    /**
     * @param stateName      the name of the state
     * @param transitions    the list of transitions to the next states
     * @param mecanumControl the mecanum wheels
     * @param gyro           the gyro sensor
     * @param velocity       the velocity to drive at
     * @param direction      the direction to drive
     * @param orientation    the angle to rotate to
     * @return the created State
     * @see MecanumControl
     * @see GyroSensor
     */
    public static State mecanumDrive(StateName stateName, List<Transition> transitions, MecanumControl mecanumControl, GyroSensor gyro, double velocity, Angle direction, Angle orientation) {
        return mecanumDrive(stateName, transitions, mecanumControl, gyro, velocity, direction, orientation, RotationControl.DEFAULT_MAX_ANGULAR_SPEED);
    }

    /**
     * drive using the mecanum wheels
     * travels for a certain amount of time defined by the robots speed and a desired distance
     *
     * @param stateName      the name of the state
     * @param nextStateName  the next state to go to
     * @param distance       the distance to travel
     * @param mecanumControl the mecanum wheels
     * @param gyro           the gyro sensor
     * @param velocity       the velocity to drive at
     * @param direction      the direction to drive
     * @param orientation    the angle to rotate to
     * @return the created State
     * @see MecanumControl
     * @see GyroSensor
     * @see Distance
     */
    public static State mecanumDrive(StateName stateName, StateName nextStateName, Distance distance, final MecanumControl mecanumControl, final GyroSensor gyro, final double velocity, final Angle direction, final Angle orientation, final Angle tolerance) {
        return mecanumDrive(stateName, nextStateName, distance, mecanumControl, gyro, velocity, direction, orientation, tolerance, RotationControl.DEFAULT_MAX_ANGULAR_SPEED);
    }

    /**
     * follow a line with the mecanum wheels
     *
     * @param stateName           the name of the state
     * @param transitions         the list of transitions to the next states
     * @param lostLineState       what state to go to if the line is lost
     * @param mecanumControl      the mecanum wheels
     * @param doubleLineSensor    the 2 line sensors
     * @param velocity            the velocity to drive at
     * @param lineFollowDirection the direction (left or right) to follow the line at
     * @return the created State
     * @see MecanumControl
     * @see TranslationControls
     */
    public State mecanumLineFollow(StateName stateName, List<Transition> transitions, StateName lostLineState, final MecanumControl mecanumControl, final DoubleLineSensor doubleLineSensor, final double velocity, final TranslationControls.LineFollowDirection lineFollowDirection) {
        transitions.add(new Transition(new EndCondition() {
            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                return !mecanumControl.translationWorked();
            }
        }, lostLineState));

        mecanumControl.setDriveMode(MecanumMotors.MecanumDriveMode.NORMALIZED);

        return new AbstractState(stateName, transitions) {
            @Override
            public void init() {
                mecanumControl.setTranslationControl(TranslationControls.lineFollow(doubleLineSensor, lineFollowDirection, velocity));
            }

            @Override
            public void run() {

            }

            @Override
            public void dispose() {
                mecanumControl.stop();
            }
        };
    }


    /**
     * Drive forward or backward with two motors
     *
     * @param stateName   the name of the state
     * @param transitions the transitions to new states
     * @param twoMotors   the motors to move
     * @param velocity    the velocity to drive at (negative for backwards)
     * @return the created State
     * @see TwoMotors
     */
    public static State drive(StateName stateName, List<Transition> transitions, final TwoMotors twoMotors, final double velocity) {
        return new AbstractState(stateName, transitions) {
            @Override
            public void init() {
                twoMotors.runMotors(velocity, velocity);
            }

            @Override
            public void run() {

            }

            @Override
            public void dispose() {
                twoMotors.runMotors(0, 0);
            }
        };
    }

    /**
     * Turn left or right
     *
     * @param stateName   the name of the state
     * @param transitions the transitions to new states
     * @param twoMotors   the motors to move
     * @param velocity    the velocity to turn at (negative for turning left)
     * @return the created State
     * @see TwoMotors
     */
    public static State turn(StateName stateName, List<Transition> transitions, final TwoMotors twoMotors, final double velocity) {
        return new AbstractState(stateName, transitions) {
            @Override
            public void init() {
                twoMotors.runMotors(velocity, -velocity);
            }

            @Override
            public void run() {

            }

            @Override
            public void dispose() {
                twoMotors.runMotors(0, 0);
            }
        };
    }

    /**
     * Turn with one wheel
     *
     * @param stateName    the name of the state
     * @param transitions  the transitions to new states
     * @param twoMotors    the motors to move
     * @param isRightWheel tells which wheel to turn
     * @param velocity     the velocity to turn the wheel at (negative for backwards)
     * @return the created State
     * @see TwoMotors
     */
    public static State oneWheelTurn(StateName stateName, List<Transition> transitions, final TwoMotors twoMotors, final boolean isRightWheel, final double velocity) {
        return new AbstractState(stateName, transitions) {
            @Override
            public void init() {
                if (isRightWheel) {
                    twoMotors.runMotors(0, velocity);
                } else {
                    twoMotors.runMotors(velocity, 0);
                }
            }

            @Override
            public void run() {

            }

            @Override
            public void dispose() {
                twoMotors.runMotors(0, 0);
            }
        };
    }


    /**
     * Drive for a certain distance
     *
     * @param stateName     the name of the state
     * @param nextStateName the state to go to after the drive is done
     * @param distance      the distance to drive
     * @param twoMotors     the motors to move
     * @param velocity      the velocity to drive at (negative for backwards)
     * @return the created State
     * @see TwoMotors
     */
    public static State drive(StateName stateName, StateName nextStateName, Distance distance, TwoMotors twoMotors, double velocity) {
        double speedMetersPerMillisecond = twoMotors.getMaxRobotSpeed().metersPerMillisecond() * velocity;
        double durationMillis = Math.abs(distance.meters() / speedMetersPerMillisecond);
        return drive(stateName, ImmutableList.of(
                new Transition(
                        EndConditions.timed((long) durationMillis),
                        nextStateName
                )
        ), twoMotors, velocity);
    }

    /**
     * Turn for a certain angle by calculating the time required for that angle
     *
     * @param stateName        the name of the state
     * @param nextStateName    the state to go to when done turning
     * @param angle            the angle to turn
     * @param minRobotTurnTime the time it takes for the robot to turn
     * @param twoMotors        the motors to run
     * @param velocity         the velocity to turn (negative for turning left)
     * @return the created State
     * @see TwoMotors
     */
    public static State turn(StateName stateName, StateName nextStateName, Angle angle, Time minRobotTurnTime, TwoMotors twoMotors, double velocity) {
        double speedRotationsPerMillisecond = velocity / minRobotTurnTime.milliseconds();
        double durationMillis = Math.abs(angle.degrees() / 360 / speedRotationsPerMillisecond);
        return turn(stateName, ImmutableList.of(
                new Transition(
                        EndConditions.timed((long) durationMillis),
                        nextStateName
                )
        ), twoMotors, velocity);
    }

    /**
     * Turn for a certain angle using a gyro sensor
     *
     * @param stateName     the name of the state
     * @param nextStateName the state to go to when done turning
     * @param angle         the angle to turn
     * @param gyro          the gyro sensor to use
     * @param twoMotors     the motors to turn
     * @param velocity      the velocity to turn at (negative to turn left)
     * @return the created State
     * @see TwoMotors
     */
    public static State turn(StateName stateName, StateName nextStateName, Angle angle, GyroSensor gyro, TwoMotors twoMotors, double velocity) {
        return turn(stateName, ImmutableList.of(
                new Transition(
                        EVEndConditions.gyroCloseToRelative(gyro, angle, Angle.fromDegrees(5)),
                        nextStateName
                )
        ), twoMotors, velocity);
    }

    /**
     * Turn with one wheel for a certain angle by calculating the time needed to turn that angle
     *
     * @param stateName        the name of the state
     * @param nextStateName    the state to go to when done turning
     * @param angle            the angle to turn
     * @param minRobotTurnTime the time it takes for the robot to turn
     * @param twoMotors        the motors to turn
     * @param isRightWheel     tells which wheel to turn
     * @param velocity         the velocity to turn the wheel at (negative for backwards)
     * @return the created State
     * @see TwoMotors
     */
    public static State oneWheelTurn(StateName stateName, StateName nextStateName, Angle angle, Time minRobotTurnTime, TwoMotors twoMotors, boolean isRightWheel, double velocity) {
        double speedRotationsPerMillisecond = velocity / minRobotTurnTime.milliseconds();
        double durationMillis = Math.abs(2 * angle.degrees() / 360 / speedRotationsPerMillisecond);
        velocity = Math.abs(velocity) * Math.signum(angle.radians());
        if (isRightWheel) {
            velocity *= -1;
        }
        return oneWheelTurn(stateName, ImmutableList.of(
                new Transition(
                        EndConditions.timed((long) durationMillis),
                        nextStateName
                )
        ), twoMotors, isRightWheel, velocity);
    }

    /**
     * Turn with one wheel for a certain angle using a gyro sensor
     *
     * @param stateName     the name of the state
     * @param nextStateName the state to go to after the turn is done
     * @param angle         the angle to turn
     * @param gyro          the gyro sensor to use
     * @param twoMotors     the motors to turn
     * @param isRightWheel  which wheel to use
     * @param velocity      the velocity to turn the wheel at (negative for backwards)
     * @return the created State
     * @see TwoMotors
     */
    public static State turn(StateName stateName, StateName nextStateName, Angle angle, GyroSensor gyro, TwoMotors twoMotors, boolean isRightWheel, double velocity) {
        velocity = Math.abs(velocity) * Math.signum(angle.radians());
        if (isRightWheel) {
            velocity *= -1;
        }
        return oneWheelTurn(stateName, ImmutableList.of(
                new Transition(
                        EVEndConditions.gyroCloseToRelative(gyro, angle, Angle.fromDegrees(5)),
                        nextStateName
                )
        ), twoMotors, isRightWheel, velocity);
    }

    /**
     * Turn a motor at a given power
     *
     * @param stateName   the name of the state
     * @param transitions the transitions to new states
     * @param motor       the motor to be turned
     * @param power       the power to turn the motor at
     * @return the created State
     * @see TwoMotors
     */
    public static State motorTurn(StateName stateName, List<Transition> transitions, final Motor motor, final double power) {
        return new AbstractState(stateName, transitions) {
            @Override
            public void init() {
                motor.setPower(power);
            }

            @Override
            public void run() {

            }

            @Override
            public void dispose() {
                motor.setPower(0);
            }
        };
    }

    /**
     * Line up with the beacon using the line sensor array and distance sensor
     *
     * @param stateName         the name of the state
     * @param successState      the state to go to if the line up succeeds
     * @param failState         the state to go to if the line up fails
     * @param mecanumControl    the mecanum wheels
     * @param direction         the direction angle to face
     * @param gyro              the gyro to use for rotation stabilization
     * @param distSensor        the distance sensor to detect distance from the beacon
     * @param lineSensorArray   the line sensor array to line up sideways with the line
     * @param teamColor         the team you are on and ...
     * @param beaconColorResult ... the beacon configuration to decide which button to line up with
     * @param distance          the distance from the beacon to line up to
     * @return the created State
     * @see LineUpControl
     */
    public static State beaconLineUp(StateName stateName, final StateName successState, final StateName failState, final MecanumControl mecanumControl, final Angle direction, final GyroSensor gyro, final DistanceSensor distSensor, final LineSensorArray lineSensorArray, TeamColor teamColor, final ResultReceiver<BeaconColorResult> beaconColorResult, final Distance distance) {
//        final EndCondition distEndCondition = EVEndConditions.distanceSensorLess(distSensor, Distance.add(distance, Distance.fromInches(4)));
        final EndCondition distEndCondition = EVEndConditions.distanceSensorLess(distSensor, distance);
        final EndCondition gyroEndCondition = EVEndConditions.gyroCloseTo(gyro, direction, 2);

        final BeaconColorResult.BeaconColor myColor = BeaconColorResult.BeaconColor.fromTeamColor(teamColor);
        final BeaconColorResult.BeaconColor opponentColor = BeaconColorResult.BeaconColor.fromTeamColor(teamColor.opposite());

        return new BasicAbstractState(stateName) {
            private boolean success;
            LineUpControl.Button buttonToLineUpWith;

            @Override
            public void init() {
                buttonToLineUpWith = null;
                if (beaconColorResult.isReady()) {
                    BeaconColorResult result = beaconColorResult.getValue();
                    BeaconColorResult.BeaconColor leftColor = result.getLeftColor();
                    BeaconColorResult.BeaconColor rightColor = result.getRightColor();
                    if (leftColor == myColor && rightColor == opponentColor) {
                        buttonToLineUpWith = LineUpControl.Button.LEFT;
                    }
                    if (leftColor == opponentColor && rightColor == myColor) {
                        buttonToLineUpWith = LineUpControl.Button.RIGHT;
                    }
                }

                success = buttonToLineUpWith != null;

                LineUpControl lineUpControl = new LineUpControl(lineSensorArray, buttonToLineUpWith, distSensor, distance, gyro, direction);

                mecanumControl.setTranslationControl(lineUpControl);
                mecanumControl.setRotationControl(lineUpControl);

                distEndCondition.init();
                gyroEndCondition.init();
            }

            @Override
            public boolean isDone() {

                if (!mecanumControl.translationWorked()) {
                    success = false;
                }
                return !success || distEndCondition.isDone();
            }

            @Override
            public StateName getNextStateName() {
                mecanumControl.stop();
                return success ? successState : failState;
            }
        };
    }

    public static State shoot(StateName stateName, final StateName nextStateName, final Shooter shooter, final int shots) {
        return new BasicAbstractState(stateName) {
            @Override
            public void init() {
                shooter.shoot(shots);
            }

            @Override
            public boolean isDone() {
//                shooter.act();
                return shooter.isDone();
            }

            @Override
            public StateName getNextStateName() {
                return nextStateName;
            }
        };
    }

    public static State gyroStabilize(StateName stateName, StateName nextStateName, final MecanumControl mecanumControl, final GyroSensor gyro, final Angle orientation, Angle tolerance) {
        List<Transition> transitions = ImmutableList.of(
                new Transition(EVEndConditions.gyroCloseTo(gyro, orientation, tolerance), nextStateName)
        );

        return new AbstractState(stateName, transitions) {
            @Override
            public void init() {
                mecanumControl.setTranslationControl(TranslationControls.ZERO);
                mecanumControl.setRotationControl(RotationControls.gyro(gyro, orientation));
            }

            @Override
            public void run() {

            }

            @Override
            public void dispose() {
                mecanumControl.stop();
            }
        };
    }

    public static State switchPressed(StateName stateName, StateName pressedStateName, StateName timeoutStateName, DigitalSensor digitalSensor, Time timeout) {
        return empty(stateName, ImmutableList.of(
                new Transition(EndConditions.inputExtractor(digitalSensor), pressedStateName),
                new Transition(EndConditions.timed((long) timeout.milliseconds()), timeoutStateName)
        ));
    }

    public static State initShooter(StateName stateName, final StateName nextStateName, final Shooter shooter) {
        return new BasicAbstractState(stateName) {
            @Override
            public void init() {
                shooter.initialize();
            }

            @Override
            public boolean isDone() {
                return shooter.isDone();
            }

            @Override
            public StateName getNextStateName() {
                return nextStateName;
            }
        };
    }


    public static State beaconColorSwitch(StateName stateName, final StateName leftButtonState, final StateName rightButtonState, final StateName unknownState, TeamColor teamColor, final ResultReceiver<BeaconColorResult> beaconColorResult) {

        final BeaconColorResult.BeaconColor myColor = BeaconColorResult.BeaconColor.fromTeamColor(teamColor);
        final BeaconColorResult.BeaconColor opponentColor = BeaconColorResult.BeaconColor.fromTeamColor(teamColor.opposite());

        return new BasicAbstractState(stateName) {
            private StateName nextState = unknownState;

            @Override
            public void init() {
                if (beaconColorResult.isReady()) {
                    BeaconColorResult result = beaconColorResult.getValue();
                    BeaconColorResult.BeaconColor leftColor = result.getLeftColor();
                    BeaconColorResult.BeaconColor rightColor = result.getRightColor();
                    if (leftColor == myColor && rightColor == opponentColor) {
                        nextState = leftButtonState;
                    }
                    if (leftColor == opponentColor && rightColor == myColor) {
                        nextState = rightButtonState;
                    }
                }

            }

            @Override
            public boolean isDone() {
                return true;
            }

            @Override
            public StateName getNextStateName() {
                return nextState;
            }
        };
    }

}

EVStates | EVEndConditions | EVStateMachineBuilder

EVEndConditions is a factory class that extends EndConditions from the state-machine-framework. It inherits all the factory methods from EndConditions, and adds its own that relate to FTC.

Some of the useful ones are:

• analogSensorGreater -- waits for an AnalogSensor to be greater than a certain value
• analogSensorLess -- waits for an AnalogSensor to be less than a certain value
• gyroCloseTo -- waits for a gyro sensor to be close to a certain value
• distanceSensorGreater -- waits for a DistanceSensor to be greater than a certain value
• distanceSensorLess -- waits for a DistanceSensor to be less than a certain value

ftc/evlib/statemachine/EVEndConditions.java

package ftc.evlib.statemachine;

import com.qualcomm.robotcore.hardware.GyroSensor;

import ftc.electronvolts.statemachine.EndCondition;
import ftc.electronvolts.statemachine.EndConditions;
import ftc.electronvolts.util.Vector2D;
import ftc.electronvolts.util.units.Angle;
import ftc.electronvolts.util.units.Distance;
import ftc.evlib.hardware.sensors.AnalogSensor;
import ftc.evlib.hardware.sensors.ColorSensor;
import ftc.evlib.hardware.sensors.DistanceSensor;
import ftc.evlib.hardware.sensors.DoubleLineSensor;
import ftc.evlib.hardware.sensors.LineFinder;
import ftc.evlib.hardware.sensors.LineSensorArray;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 5/10/16
 * <p>
 * Factory class for EndCondition
 * extends EndConditions, which has some useful factory methods already
 *
 * @see EndCondition
 * @see EndConditions
 */
public class EVEndConditions extends EndConditions {

    /**
     * wait for a line of any color
     *
     * @param doubleLineSensor   the two line sensors
     * @param detectionsRequired the number of detections in a row to look for (to avoid false positives)
     * @return the created EndCondition
     * @see DoubleLineSensor
     */
    public static EndCondition foundLine(final DoubleLineSensor doubleLineSensor, final int detectionsRequired) {
        return new EndCondition() {
            int detections;

            @Override
            public void init() {
                detections = 0;
            }

            @Override
            public boolean isDone() {
                DoubleLineSensor.LinePosition linePosition = doubleLineSensor.getPosition();
                if (linePosition == DoubleLineSensor.LinePosition.LEFT || linePosition == DoubleLineSensor.LinePosition.MIDDLE || linePosition == DoubleLineSensor.LinePosition.RIGHT) {
                    detections++;
                    if (detections > detectionsRequired) {
                        return true;
                    }
                } else {
                    detections = 0;
                }
                return false;
            }
        };
    }

    /**
     * wait for a line of a certain color
     *
     * @param lineFinder         two line sensors and a reflective color sensor
     * @param lineColor          the color of line to look for
     * @param detectionsRequired the number of detections in a row to look for (to avoid false positives)
     * @return the created EndCondition
     * @see LineFinder
     */
    public static EndCondition foundColoredLine(final LineFinder lineFinder, final LineFinder.LineColor lineColor, final int detectionsRequired) {
        return new EndCondition() {
            int detections;

            @Override
            public void init() {
                detections = 0;
                lineFinder.startLookingFor(lineColor);
            }

            @Override
            public boolean isDone() {
                if (lineFinder.getValue()) {
                    detections++;
                    if (detections > detectionsRequired) {
                        return true;
                    }
                } else {
                    detections = 0;
                }
                return false;
            }
        };
    }

    /**
     * look for a value from a sensor that is greater/less than a target value
     *
     * @param analogSensor the sensor
     * @param target       the target sensor value
     * @param greater      true if the sensor value needs to be greater than the target value
     * @return the created EndCondition
     * @see AnalogSensor
     */
    public static EndCondition analogSensor(final AnalogSensor analogSensor, final double target, final boolean greater) {
        return new EndCondition() {
            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                if (greater) {
                    return (analogSensor.getValue() >= target);
                } else {
                    return (analogSensor.getValue() <= target);
                }
            }
        };
    }

    /**
     * look for a value from a sensor that is greater than a target value
     *
     * @param analogSensor the sensor
     * @param value        the target value
     * @return the created EndCondition
     * @see AnalogSensor
     */
    public static EndCondition analogSensorGreater(AnalogSensor analogSensor, double value) {
        return analogSensor(analogSensor, value, true);
    }

    /**
     * look for a value from a sensor that is less than a target value
     *
     * @param analogSensor the sensor
     * @param value        the target value
     * @return the created EndCondition
     * @see AnalogSensor
     */
    public static EndCondition analogSensorLess(AnalogSensor analogSensor, double value) {
        return analogSensor(analogSensor, value, false);
    }

    /**
     * wait until the gyro heading is close to a target value
     *
     * @param gyro             the gyro sensor
     * @param targetDegrees    the target value (in degrees)
     * @param toleranceDegrees the accepted tolerance to be considered "close to" (in degrees)
     * @return the created EndCondition
     * @see GyroSensor
     */
    public static EndCondition gyroCloseTo(GyroSensor gyro, double targetDegrees, double toleranceDegrees) {
        return gyroCloseTo(gyro, Angle.fromDegrees(targetDegrees), Angle.fromDegrees(toleranceDegrees));
    }

    /**
     * wait until the gyro heading is close to a target value
     *
     * @param gyro             the gyro sensor
     * @param target           the target value
     * @param toleranceDegrees the accepted tolerance to be considered "close to" (in degrees)
     * @return the created EndCondition
     * @see GyroSensor
     */
    public static EndCondition gyroCloseTo(GyroSensor gyro, Angle target, double toleranceDegrees) {
        return gyroCloseTo(gyro, target, Angle.fromDegrees(toleranceDegrees));
    }

    /**
     * wait until the gyro heading is close to a target value
     *
     * @param gyro          the gyro sensor
     * @param targetDegrees the target value (in degrees)
     * @param tolerance     the accepted tolerance to be considered "close to"
     * @return the created EndCondition
     * @see GyroSensor
     */
    public static EndCondition gyroCloseTo(GyroSensor gyro, double targetDegrees, Angle tolerance) {
        return gyroCloseTo(gyro, Angle.fromDegrees(targetDegrees), tolerance);
    }

    /**
     * wait until the gyro heading is close to a target value
     *
     * @param gyro      the gyro sensor
     * @param target    the target value
     * @param tolerance the accepted tolerance to be considered "close to"
     * @return the created EndCondition
     * @see GyroSensor
     */
    public static EndCondition gyroCloseTo(final GyroSensor gyro, Angle target, final Angle tolerance) {
        final Vector2D targetVector = new Vector2D(1, target);
        return new EndCondition() {
            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                Vector2D gyroVector = new Vector2D(1, Angle.fromDegrees(gyro.getHeading()));
                Angle separation = Vector2D.signedAngularSeparation(targetVector, gyroVector);
                return Math.abs(separation.radians()) <= tolerance.radians();
            }
        };
    }

    /**
     * wait until the gyro heading is close to a target value relative to the starting heading
     *
     * @param gyro      the gyro sensor
     * @param target    the target value relative to the starting heading
     * @param tolerance the accepted tolerance to be considered "close to"
     * @return the created State
     * @see GyroSensor
     */
    public static EndCondition gyroCloseToRelative(final GyroSensor gyro, Angle target, final Angle tolerance) {
        final Vector2D targetVector = new Vector2D(1, target);
        return new EndCondition() {
            double gyroInit = 0;

            @Override
            public void init() {
                gyroInit = gyro.getHeading();
            }

            @Override
            public boolean isDone() {
                Vector2D gyroVector = new Vector2D(1, Angle.fromDegrees(gyro.getHeading() - gyroInit));
                Angle separation = Vector2D.signedAngularSeparation(targetVector, gyroVector);
                return Math.abs(separation.degrees()) <= tolerance.degrees();
            }
        };
    }

    /**
     * wait for a reflection of red light with a color sensor
     *
     * @param colorSensor the sensor
     * @return the created EndCondition
     * @see ColorSensor
     */
    public static EndCondition colorSensorRedLight(final ColorSensor colorSensor) {
        return new EndCondition() {
            @Override
            public void init() {
                colorSensor.setColorRed();
            }

            @Override
            public boolean isDone() {
                return colorSensor.getValue();
            }
        };
    }

    /**
     * wait for a reflection of blue light with a color sensor
     *
     * @param colorSensor the sensor
     * @return the created EndCondition
     * @see ColorSensor
     */
    public static EndCondition colorSensorBlueLight(final ColorSensor colorSensor) {
        return new EndCondition() {
            @Override
            public void init() {
                colorSensor.setColorBlue();
            }

            @Override
            public boolean isDone() {
                return colorSensor.getValue();
            }
        };
    }

    /**
     * Wait for any sensor to activate on a line sensor array
     *
     * @param lineSensorArray the sensor
     * @return the created State
     * @see LineSensorArray
     */
    public static EndCondition lineSensorArrayAny(final LineSensorArray lineSensorArray) {
        return new EndCondition() {
            @Override
            public void init() {

            }

            @Override
            public boolean isDone() {
                return lineSensorArray.getNumSensorsActive() > 0;
            }
        };
    }

    /**
     * Wait for a distance sensor to cross a threshold
     *
     * @param distanceSensor the distance sensor
     * @param target         the target value
     * @param greater        whether to wait for the sensor to be greater or less than the target
     * @return the created State
     * @see DistanceSensor
     */
    public static EndCondition distanceSensor(final DistanceSensor distanceSensor, final Distance target, final boolean greater) {
        return new EndCondition() {
            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                if (greater) {
                    return (distanceSensor.getDistance().meters() >= target.meters());
                } else {
                    return (distanceSensor.getDistance().meters() <= target.meters());
                }
            }
        };
    }

    /**
     * Wait for a distance sensor to be less than a certain distance
     *
     * @param distanceSensor the distance sensor
     * @param target         the target Distance
     * @return the created State
     * @see DistanceSensor
     */
    public static EndCondition distanceSensorLess(DistanceSensor distanceSensor, Distance target) {
        return distanceSensor(distanceSensor, target, false);
    }

    /**
     * Wait for a distance sensor to be greater than a certain distance
     *
     * @param distanceSensor the distance sensor
     * @param target         the target Distance
     * @return the created State
     * @see DistanceSensor
     */
    public static EndCondition distanceSensorGreater(DistanceSensor distanceSensor, Distance target) {
        return distanceSensor(distanceSensor, target, true);
    }
}

EVStates | EVEndConditions | EVStateMachineBuilder

EVStateMachineBuilder is a builder class that extends StateMachineBuilder from the state-machine-framework. It inherits all the convenience methods from StateMachineBuilder, and adds its own that relate to FTC.

When you create the EVStateMachineBuilder, you pass in servos, sensors, motors, etc. Then you can call the convenience methods to add servo states, drive states, etc. without passing in the objects again. If you don't have one of the object, a gyro for example, you can pass in a null value and not use any of the gyro convenience methods.

ftc/evlib/statemachine/EVStateMachineBuilder.java

package ftc.evlib.statemachine;

import com.qualcomm.robotcore.hardware.GyroSensor;

import java.util.List;

import ftc.electronvolts.statemachine.StateMachineBuilder;
import ftc.electronvolts.statemachine.StateName;
import ftc.electronvolts.statemachine.Transition;
import ftc.electronvolts.util.TeamColor;
import ftc.electronvolts.util.units.Angle;
import ftc.electronvolts.util.units.Distance;
import ftc.evlib.hardware.control.MecanumControl;
import ftc.evlib.hardware.mechanisms.Shooter;
import ftc.evlib.hardware.sensors.DistanceSensor;
import ftc.evlib.hardware.servos.ServoControl;
import ftc.evlib.hardware.servos.ServoName;
import ftc.evlib.hardware.servos.Servos;
import ftc.evlib.vision.framegrabber.FrameGrabber;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 5/10/16
 * <p>
 * Builder that uses the EVStates Factory class to create states and build them into a StateMachine
 * extends StateMachineBuilder which has the basic builder methods as well as some useful addXYZ methods
 *
 * @see EVStates
 * @see StateMachineBuilder
 */
public class EVStateMachineBuilder extends StateMachineBuilder {
    private final TeamColor teamColor;
    private final Angle tolerance;
    private final MecanumControl mecanumControl;
    private final GyroSensor gyro;
    private final FrameGrabber frameGrabber;
    private final Servos servos;
    private final DistanceSensor distanceSensor;
    private final Shooter shooter;

    /**
     * any of the parameters can be null if the robot does not have it
     *
     * @param firstStateName the state to start with
     * @param teamColor      the alliance you are on
     * @param tolerance      the tolerance on gyro angles
     * @param gyro           the gyro sensor
     * @param frameGrabber   access to the camera
     * @param servos         the servos
     * @param distanceSensor the distance sensor
     * @param mecanumControl the mecanum wheel controller
     * @param shooter        the helper class for the mecanism that shoots the particles
     */
    public EVStateMachineBuilder(StateName firstStateName, TeamColor teamColor, Angle tolerance, GyroSensor gyro, FrameGrabber frameGrabber, Servos servos, DistanceSensor distanceSensor, MecanumControl mecanumControl, Shooter shooter) {
        super(firstStateName);
        this.teamColor = teamColor;
        this.tolerance = tolerance;
        this.mecanumControl = mecanumControl;
        this.gyro = gyro;
        this.frameGrabber = frameGrabber;
        this.servos = servos;
        this.distanceSensor = distanceSensor;
        this.shooter = shooter;
    }

    public EVStateMachineBuilder(StateName firstStateName, EVStateMachineBuilder b) {
        this(firstStateName, b.teamColor, b.tolerance, b.gyro, b.frameGrabber, b.servos, b.distanceSensor, b.mecanumControl, b.shooter);
    }

//    public void addCameraTracking(StateName stateName, StateName doneState, StateName lostObjectState, long timeoutMillis, StateName timeoutState, ImageProcessor<? extends Location> imageProcessor) {
//        add(EVStates.mecanumCameraTrack(stateName, doneState, lostObjectState, timeoutMillis, timeoutState, mecanumControl, frameGrabber, imageProcessor));
//    }

    //convenience methods for adding different types of States

    public void addServoInit(StateName stateName, StateName nextStateName) {
        add(EVStates.servoInit(stateName, nextStateName, servos));
    }

    public void addCalibrateGyro(StateName stateName, StateName nextStateName) {
        add(EVStates.calibrateGyro(stateName, nextStateName, gyro));
    }

    public void addStop(StateName stateName) {
        add(EVStates.stop(stateName, mecanumControl));
    }

    ///// START DRIVE STATES /////
    public void addDrive(StateName stateName, List<Transition> transitions, double velocity, double directionDegrees, double orientationDegrees, double maxAngularSpeed) {
        add(EVStates.mecanumDrive(stateName, transitions, mecanumControl, gyro, velocity, Angle.fromDegrees(directionDegrees), Angle.fromDegrees(orientationDegrees), maxAngularSpeed));
    }

    public void addDrive(StateName stateName, List<Transition> transitions, double velocity, double directionDegrees, double orientationDegrees) {
        add(EVStates.mecanumDrive(stateName, transitions, mecanumControl, gyro, velocity, Angle.fromDegrees(directionDegrees), Angle.fromDegrees(orientationDegrees)));
    }

    public void addDrive(StateName stateName, List<Transition> transitions, double velocity, Angle direction, Angle orientation, double maxAngularSpeed) {
        add(EVStates.mecanumDrive(stateName, transitions, mecanumControl, gyro, velocity, direction, orientation, maxAngularSpeed));
    }

    public void addDrive(StateName stateName, List<Transition> transitions, double velocity, Angle direction, Angle orientation) {
        add(EVStates.mecanumDrive(stateName, transitions, mecanumControl, gyro, velocity, direction, orientation));
    }


    public void addDrive(StateName stateName, StateName nextStateName, Distance distance, double velocity, double directionDegrees, double orientationDegrees, double maxAngularSpeed) {
        add(EVStates.mecanumDrive(stateName, nextStateName, distance, mecanumControl, gyro, velocity, Angle.fromDegrees(directionDegrees), Angle.fromDegrees(orientationDegrees), tolerance, maxAngularSpeed));
    }

    public void addDrive(StateName stateName, StateName nextStateName, Distance distance, double velocity, double directionDegrees, double orientationDegrees) {
        add(EVStates.mecanumDrive(stateName, nextStateName, distance, mecanumControl, gyro, velocity, Angle.fromDegrees(directionDegrees), Angle.fromDegrees(orientationDegrees), tolerance));
    }

    public void addDrive(StateName stateName, StateName nextStateName, Distance distance, double velocity, Angle direction, Angle orientation, double maxAngularSpeed) {
        add(EVStates.mecanumDrive(stateName, nextStateName, distance, mecanumControl, gyro, velocity, direction, orientation, tolerance, maxAngularSpeed));
    }

    public void addDrive(StateName stateName, StateName nextStateName, Distance distance, double velocity, Angle direction, Angle orientation) {
        add(EVStates.mecanumDrive(stateName, nextStateName, distance, mecanumControl, gyro, velocity, direction, orientation, tolerance));
    }

    public void addDrive(StateName stateName, StateName nextStateName, Distance distance, double velocity, Angle direction, Angle orientation, Angle tolerance, double maxAngularSpeed) {
        add(EVStates.mecanumDrive(stateName, nextStateName, distance, mecanumControl, gyro, velocity, direction, orientation, tolerance, maxAngularSpeed));
    }

    public void addDrive(StateName stateName, StateName nextStateName, Distance distance, double velocity, Angle direction, Angle orientation, Angle tolerance) {
        add(EVStates.mecanumDrive(stateName, nextStateName, distance, mecanumControl, gyro, velocity, direction, orientation, tolerance));
    }
    ///// END DRIVE STATES /////

    ///// START TURN STATES /////
    public void addGyroTurn(StateName stateName, List<Transition> transitions, double orientationDegrees, double maxAngularSpeed) {
        add(EVStates.mecanumDrive(stateName, transitions, mecanumControl, gyro, 0, Angle.zero(), Angle.fromDegrees(orientationDegrees), maxAngularSpeed));
    }

    public void addGyroTurn(StateName stateName, List<Transition> transitions, double orientationDegrees) {
        add(EVStates.mecanumDrive(stateName, transitions, mecanumControl, gyro, 0, Angle.zero(), Angle.fromDegrees(orientationDegrees)));
    }

    public void addGyroTurn(StateName stateName, List<Transition> transitions, Angle orientation, double maxAngularSpeed) {
        add(EVStates.mecanumDrive(stateName, transitions, mecanumControl, gyro, 0, Angle.zero(), orientation, maxAngularSpeed));
    }

    public void addGyroTurn(StateName stateName, List<Transition> transitions, Angle orientation) {
        add(EVStates.mecanumDrive(stateName, transitions, mecanumControl, gyro, 0, Angle.zero(), orientation));
    }

    public void addGyroTurn(StateName stateName, StateName nextStateName, double orientationDegrees, double toleranceDegrees, double maxAngularSpeed) {
        add(EVStates.mecanumDrive(stateName, nextStateName, Distance.zero(), mecanumControl, gyro, 0, Angle.zero(), Angle.fromDegrees(orientationDegrees), Angle.fromDegrees(toleranceDegrees), maxAngularSpeed));
    }

    public void addGyroTurn(StateName stateName, StateName nextStateName, double orientationDegrees, double toleranceDegrees) {
        add(EVStates.mecanumDrive(stateName, nextStateName, Distance.zero(), mecanumControl, gyro, 0, Angle.zero(), Angle.fromDegrees(orientationDegrees), Angle.fromDegrees(toleranceDegrees)));
    }

    public void addGyroTurn(StateName stateName, StateName nextStateName, Angle orientation, Angle tolerance, double maxAngularSpeed) {
        add(EVStates.mecanumDrive(stateName, nextStateName, Distance.zero(), mecanumControl, gyro, 0, Angle.zero(), orientation, tolerance, maxAngularSpeed));
    }

    public void addGyroTurn(StateName stateName, StateName nextStateName, Angle orientation, Angle tolerance) {
        add(EVStates.mecanumDrive(stateName, nextStateName, Distance.zero(), mecanumControl, gyro, 0, Angle.zero(), orientation, tolerance));
    }
    ///// END TURN STATES /////

    ///// START SERVO STATES /////
    private ServoControl getServo(ServoName servoName) {
        if (!servos.getServoMap().containsKey(servoName)) {
            throw new IllegalArgumentException("ServoName \"" + servoName + "\" was not found in the servoMap");
        }
        return servos.getServoMap().get(servoName);

    }

    public void addServo(StateName stateName, StateName nextStateName, ServoName servoName, double position, boolean waitForDone) {
        add(EVStates.servoTurn(stateName, nextStateName, getServo(servoName), position, waitForDone));
    }

    public void addServo(StateName stateName, StateName nextStateName, ServoName servoName, double position, double speed, boolean waitForDone) {
        add(EVStates.servoTurn(stateName, nextStateName, getServo(servoName), position, speed, waitForDone));
    }

    public void addServo(StateName stateName, StateName nextStateName, ServoName servoName, Enum preset, boolean waitForDone) {
        add(EVStates.servoTurn(stateName, nextStateName, getServo(servoName), preset, waitForDone));
    }

    public void addServo(StateName stateName, StateName nextStateName, ServoName servoName, Enum preset, double speed, boolean waitForDone) {
        add(EVStates.servoTurn(stateName, nextStateName, getServo(servoName), preset, speed, waitForDone));
    }
    ///// END SERVO STATES /////

    public void addTelem(StateName stateName, String message, double value, List<Transition> transitions) {
        add(EVStates.telemetry(stateName, transitions, message, value));
    }

    public void addTelem(StateName stateName, String message, double value) {
        add(EVStates.telemetry(stateName, message, value));
    }

//    public void addBeaconLineUp(StateName stateName, StateName successState, StateName failState, Angle direction, ResultReceiver<BeaconColorResult> beaconColorResult, Distance distance) {
//        add(EVStates.beaconLineUp(stateName, successState, failState, mecanumControl, direction, gyro, distanceSensor, lineSensorArray, teamColor, beaconColorResult, distance));
//    }

    public void addShoot(StateName stateName, StateName nextStateName, int shots) {
        add(EVStates.shoot(stateName, nextStateName, shooter, shots));
    }
}

In Java, an interface sets requirements for the classes that implement it. The methods on the interface have no code that executes, but they tell what the method name, parameters, and return type will be.

The State interface says that each implementation of the State interface must have an act() method which returns the name of the state to be run next call of the loop method. It can return its own name or the name of another state if the current state is done. It also has to have a getName() method which returns the State's StateName. The state interface is used for all other states, including BasicAbstractState and AbstractState.

ftc/electronvolts/statemachine/State.java

package ftc.electronvolts.statemachine;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * This is the core of the state machine framework. It is the interface that
 * defines the required behavior of all the classes that call themselves a
 * "State". This way anything that receives a State object knows exactly what
 * methods that object has, regardless of the state's internal structure or
 * function.
 */
public interface State {
    /**
     * Act is run in every single loop
     *
     * @return Returns the name of the next state. Returns null to stay
     *         in the current state.
     */
    StateName act();
}

The next step in the guide is to write a Simple State Machine.

Let's say you have a state machine drawing that looks like this:

 _______        ____________        ______
|       \      |            |      /      \
| START  \ ==> |  wait for  | ==> |  STOP  |
|        /     |  1 second  |     |        |
|_______/      |____________|      \______/

To create a state machine based on this diagram, first create an enum that implements [StateName](The State Interface.md) with your list of states:

enum S implements StateName {
    WAIT,
    STOP
};

Then create a StateMachineBuilder, giving it the state to start with:

StateMachineBuilder b = new StateMachineBuilder(S.WAIT);

Add all the states to the builder:

//add a wait state named WAIT for 1000 milliseconds, then go to the state named STOP
b.addWait(S.WAIT, S.STOP, 1000);

//add a stop state named STOP
b.addStop(S.STOP);

Build the state machine and save it to a variable.

StateMachine stateMachine = b.build();

Add the following to a loop method to run the state machine:

stateMachine.act();

//use this for the FTC app:
telemetry.addData("State", stateMachine.getCurrentStateName());

//use this for other usages:
System.out.println("State: " + stateMachine.getCurrentStateName());

On the telemetry of the FTC app, or the console in other applications, this will be displayed:

State: WAIT
State: WAIT
State: WAIT
State: WAIT
...
State: WAIT
State: WAIT
State: STOP
State: STOP
State: STOP
State: STOP
...

Congratulations! You made your first state machine!

The next step is to create Custom States.

First, create a class that extends the States factory class. It will inherit all the methods from States.

public class MyStates extends States {

}

You can add methods to it such as a gyro calibration state or a telemetry state:

public class MyStates extends States {
    public static State calibrateGyro(StateName stateName, final GyroSensor gyro, final StateName nextStateName){
        gyro.calibrate();
        return new BasicAbstractState(stateName) {
            @Override
            public void init() {}

            @Override
            public boolean isDone() {
                return !gyro.isCalibrating();
            }

            @Override
            public StateName getNextStateName() {
                return nextStateName;
            }
        };
    }
    
    
    public static State telemetry(Map<StateName, EndCondition> transitions, final String message, final double value) {
        return new AbstractState(transitions) {
            @Override
            public void init() {
            }

            @Override
            public void run() {
                telemetry.addData(message, value);
            }

            @Override
            public void dispose() {
            }
        };
    }

The next step is to create a Custom StateMachineBuilder.

Make a class that extends StateMachineBuilder and calls the superconstructor.

public class MyStateMachineBuilder extends StateMachineBuilder {
    public MyStateMachineBuilder(StateName firstStateName) {
        super(firstStateName);
    }
}

Then add a convenience method to it that uses your method you made in [MyStates](Custom States.md).

public class MyStateMachineBuilder extends StateMachineBuilder {
    public MyStateMachineBuilder(StateName firstStateName) {
        super(firstStateName);
    }
    
    public void addCalibrateGyro(StateName stateName, GyroSensor gyro, StateName nextStateName){
        add(stateName, MyStates.calibrateGyro(gyro, nextStateName));
    }
    
    public void addTelem(StateName stateName, String message, double value, Map<StateName, EndCondition> transitions) {
        add(stateName, EVStates.telemetry(transitions, message, value));
    }
}

You can even pass in objects in the constructor to be used multiple times, such as the gyro sensor or telemetry.

public class MyStateMachineBuilder extends StateMachineBuilder {
    private final Telemetry telemetry;
    
    public MyStateMachineBuilder(StateName firstStateName, Telemetry telemetry) {
        super(firstStateName);
        this.telemetry = telemetry;
    }
    
    public void addCalibrateGyro(StateName stateName, GyroSensor gyro, StateName nextStateName){
        add(stateName, MyStates.calibrateGyro(gyro, nextStateName));
    }
    
    public void addTelem(StateName stateName, String message, double value, Map<StateName, EndCondition> transitions) {
        add(stateName, EVStates.telemetry(transitions, message, value));
    }
}

The next step is to create Custom EndConditions.

This page is out of date (unsupported feature)

To create your own EndConditions, all you have to do is make a class that extends EndConditions.

public class EVEndConditions extends EndConditions {

}

And add some methods to it, for example a gyro end condition.

public class EVEndConditions extends EndConditions {

    public static EndCondition gyroCloseTo(final GyroSensor gyro, double targetDegrees, final double toleranceDegrees){
        final Vector3D targetVector = Vector3D.fromPolar2D(1, Angle.fromDegrees(targetDegrees));
        return new EndCondition() {
            @Override
            public void init() {}

            @Override
            public boolean isDone() {
                Vector3D gyroVector = Vector3D.fromPolar2D(1, Angle.fromDegrees(gyro.getHeading()));
                Angle separation = Vector3D.signedAngularSeparation(targetVector, gyroVector);
                return Math.abs(separation.getValueDegrees()) <= toleranceDegrees;
            }
        };
    }

}

This page is out of date (unsupported feature)

BasicAbstractState is a basic implementation of the State interface that separates the act() method into three methods: init(), isDone(), and getNextStateName().

• The init() method is called the first time the state is run. You can reset timers, turn on motors, etc.
• The isDone() method is called every loop after init(). You can check sensors, timers, or encoders, and return true when the state needs to exit.
• The getNextStateName() method is called after the state is done (when the isDone() method returns true). Here you can decide what state to enter next, or have one state that is always next.

Examples of BasicAbstractState (and AbstractState) are found in the States factory class.

ftc/electronvolts/statemachine/BasicAbstractState.java

package ftc.electronvolts.statemachine;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * BasicAbstractState removes the need to use transitions. It requires that the
 * state manages when it should be completed. It is used when the end condition
 * is directly linked to the state's action, such as a state that turns a servo
 * and exits when the servo is done, or a state that detects red or blue using a
 * color sensor and activates a different state based on the result.
 */
public abstract class BasicAbstractState implements State {
    private boolean isRunning = false;

    /**
     * Implementation of the act() method in the State interface
     * 
     * Runs init() on the first call of the act() method, then isDone() on
     * subsequent calls. If isDone() returns true, getNextStateName is called
     * and the value from it is returned as the next state
     * 
     * @return The name of the state to be run next cycle (returns null if there
     *         are no state changes)
     */
    @Override
    public StateName act() {
        if (!isRunning) {
            init();
            isRunning = true;
        }
        if (isDone()) {
            isRunning = false;
            return getNextStateName();
        }
        return null;
    }

    /**
     * Run once when the state is initialized This can do tasks such as setting
     * servo positions or turning on LED's
     */
    public abstract void init();

    /**
     * Run every cycle after init() This can do tasks such as checking if a
     * sensor is calibrated, updating a value, exiting right away or even never
     * exiting at all.
     *
     * @return true if state is finished executing
     */
    public abstract boolean isDone();

    /**
     * The possible next states are passed in through the constructor, and one
     * of them is chosen here. This lets the state define how the decision is
     * made about the next state. It could have any number of next states for
     * any number of reasons, for example: 0 next states for a stop state, 1
     * next state for a servo turn state, 3 next states for a detect color state
     * (red, blue, or unknown)
     *
     * @return the name of the next state to be executed
     */
    public abstract StateName getNextStateName();
}

This page is out of date (unsupported feature)

This is an interface used to tell when a state is done. Every implementation of it must have an “init” method, where sensors or motors can be reset, and values can be initialized. It also must have an “isDone” method, where it can read the sensors, motors, etc., and report back to tell if the condition has been met. EndCondition objects are created in the EndConditions factory class, and used in the AbstractState class with a StateMap.

ftc/electronvolts/statemachine/EndCondition.java

package ftc.electronvolts.statemachine;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * An EndCondition is used as a condition to tell when something is done. If the
 * end condition is true, the next state is run.
 */
public interface EndCondition {
    /**
     * Run once when the end condition is created Can be used to tell helper
     * classes to initialize Be careful to reset all variables here in case the
     * state is run a second time
     */
    void init();

    /**
     * Run every cycle after init() Used to get values from helper classes to
     * see if the condition is met
     *
     * @return true if condition is met.
     */
    boolean isDone();
}

This page is out of date (unsupported feature)

The StateMap class stores the next StateNames and their corresponding EndConditions. It has several of() methods for easily creating StateMap objects. It is used in AbstractState to keep track of the transitions out of a state.

ftc/electronvolts/statemachine/StateMap.java

package ftc.electronvolts.statemachine;

import java.util.HashMap;
import java.util.Map;

/**
 * This file was made by the electronVolts, FTC team 7393
 */
public class StateMap extends HashMap<StateName, EndCondition> {
    private static final long serialVersionUID = 3897898537401872088L;

    public static class Entry implements Map.Entry<StateName, EndCondition> {
        private final StateName key;
        private EndCondition value;

        public Entry(StateName key, EndCondition value) {
            this.key = key;
            this.value = value;
        }

        @Override
        public StateName getKey() {
            return key;
        }

        @Override
        public EndCondition getValue() {
            return value;
        }

        @Override
        public EndCondition setValue(EndCondition value) {
            EndCondition old = this.value;
            this.value = value;
            return old;
        }
    }

    public static StateMap of() {
        return new StateMap();
    }

    public static StateMap of(StateName k1, EndCondition v1) {
        return of(new Entry(k1, v1));
    }

    public static StateMap of(StateName k1, EndCondition v1, StateName k2, EndCondition v2) {
        return of(new Entry(k1, v1), new Entry(k2, v2));
    }

    public static StateMap of(StateName k1, EndCondition v1, StateName k2, EndCondition v2, StateName k3, EndCondition v3) {
        return of(new Entry(k1, v1), new Entry(k2, v2), new Entry(k3, v3));
    }

    public static StateMap of(StateName k1, EndCondition v1, StateName k2, EndCondition v2, StateName k3, EndCondition v3, StateName k4, EndCondition v4) {
        return of(new Entry(k1, v1), new Entry(k2, v2), new Entry(k3, v3), new Entry(k4, v4));
    }

    public static StateMap of(StateName k1, EndCondition v1, StateName k2, EndCondition v2, StateName k3, EndCondition v3, StateName k4, EndCondition v4, StateName k5, EndCondition v5) {
        return of(new Entry(k1, v1), new Entry(k2, v2), new Entry(k3, v3), new Entry(k4, v4), new Entry(k5, v5));
    }

    public static StateMap of(StateName k1, EndCondition v1, StateName k2, EndCondition v2, StateName k3, EndCondition v3, StateName k4, EndCondition v4, StateName k5, EndCondition v5, StateName k6, EndCondition v6) {
        return of(new Entry(k1, v1), new Entry(k2, v2), new Entry(k3, v3), new Entry(k4, v4), new Entry(k5, v5), new Entry(k5, v5), new Entry(k6, v6));
    }

    public static StateMap of(StateName k1, EndCondition v1, StateName k2, EndCondition v2, StateName k3, EndCondition v3, StateName k4, EndCondition v4, StateName k5, EndCondition v5, StateName k6, EndCondition v6, StateName k7, EndCondition v7) {
        return of(new Entry(k1, v1), new Entry(k2, v2), new Entry(k3, v3), new Entry(k4, v4), new Entry(k5, v5), new Entry(k5, v5), new Entry(k6, v6), new Entry(k7, v7));
    }

    public static StateMap of(StateName k1, EndCondition v1, StateName k2, EndCondition v2, StateName k3, EndCondition v3, StateName k4, EndCondition v4, StateName k5, EndCondition v5, StateName k6, EndCondition v6, StateName k7, EndCondition v7, StateName k8, EndCondition v8) {
        return of(new Entry(k1, v1), new Entry(k2, v2), new Entry(k3, v3), new Entry(k4, v4), new Entry(k5, v5), new Entry(k5, v5), new Entry(k6, v6), new Entry(k7, v7), new Entry(k8, v8));
    }

    public static StateMap of(StateName k1, EndCondition v1, StateName k2, EndCondition v2, StateName k3, EndCondition v3, StateName k4, EndCondition v4, StateName k5, EndCondition v5, StateName k6, EndCondition v6, StateName k7, EndCondition v7, StateName k8, EndCondition v8, StateName k9, EndCondition v9) {
        return of(new Entry(k1, v1), new Entry(k2, v2), new Entry(k3, v3), new Entry(k4, v4), new Entry(k5, v5), new Entry(k5, v5), new Entry(k6, v6), new Entry(k7, v7), new Entry(k8, v8), new Entry(k9, v9));
    }

    public static StateMap of(StateName k1, EndCondition v1, StateName k2, EndCondition v2, StateName k3, EndCondition v3, StateName k4, EndCondition v4, StateName k5, EndCondition v5, StateName k6, EndCondition v6, StateName k7, EndCondition v7, StateName k8, EndCondition v8, StateName k9, EndCondition v9, StateName k10, EndCondition v10) {
        return of(new Entry(k1, v1), new Entry(k2, v2), new Entry(k3, v3), new Entry(k4, v4), new Entry(k5, v5), new Entry(k5, v5), new Entry(k6, v6), new Entry(k7, v7), new Entry(k8, v8), new Entry(k9, v9), new Entry(k10, v10));
    }

    /**
     * @throws IllegalArgumentException if separate entries have the same key
     */
    public static StateMap of(Entry... entries) {
        StateMap stateMap = new StateMap();
        for (Entry entry : entries) {
            if (stateMap.containsKey(entry.key)) {
                throw new IllegalArgumentException("Separate entries cannot have the same key.");
            }
            stateMap.put(entry.key, entry.value);
        }
        return stateMap;
    }
}

This page is out of date (unsupported feature)

We have established that all states need an "act" method, but what if the state needs a method that runs once before the main loop starts or a method that runs after the state ends? And what about moving from one state to the next?

AbstractState is a more complex (than BasicAbstractState) implementation of the State interface that separates the State's functionality from the transitions to other states. It separates the act() method into three methods: init(), run(), and dispose().

• The init() method is called the first time the state is run. You can reset values, turn on motors, etc.
• The run() method is called every loop after init(). You can update motor powers, display values on telemetry, etc.
• The dispose() method is called after the state is done. You can turn off motors, close files, etc.

The way AbstractState handles the transitions to other states is by using StateMap. When it is created, a map of StateName to EndCondition is passed in.

You can see examples of AbstractState (and BasicAbstractState) in the States factory class.

ftc/electronvolts/statemachine/AbstractState.java

package ftc.electronvolts.statemachine;

import java.util.Map;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * AbstractState is a simple state that handles transitions.
 */
public abstract class AbstractState implements State {
    // Map of possible transitions to other states
    private final Map<StateName, EndCondition> transitions;

    private boolean isRunning = false;

    /**
     * An abstract state must contain a map of transitions, containing end
     * conditions and their respective next state names.
     *
     * @param transitions the map of EndCondition to NextState
     * @see Transition
     */
    public AbstractState(Map<StateName, EndCondition> transitions) {
        this.transitions = transitions;
    }


    /**
     * Implementation of the act() method in the State interface
     * Cycles through Transitions to run one of the next states if its
     * EndCondition has been met. Does "edge detection" on the state, running
     * init() for the first cycle, run() for subsequent cycles, and dispose()
     * for the last cycle.
     *
     * @return The name of the state to be run next cycle (returns itself if
     *         there are no state changes)
     */
    @Override
    public StateName act() {
        if (!isRunning) {
            init();
            isRunning = true;
            for(Map.Entry<StateName, EndCondition> entry : transitions.entrySet()) {
                EndCondition endCondition = entry.getValue();
                endCondition.init();
            }
        }
        for(Map.Entry<StateName, EndCondition> entry : transitions.entrySet()) {
            StateName stateName = entry.getKey();
            EndCondition endCondition = entry.getValue();
            if (endCondition.isDone()) {
                dispose();
                isRunning = false;
                return stateName;
            }
        }
        run();
        return null;
    }

    /**
     * Run once when the state is initialized.
     * This can run tasks such as starting motors.
     */
    public abstract void init();

    /**
     * Run every cycle after init()
     * This can do tasks such as gyro stabilization or line following
     */
    public abstract void run();

    /**
     * Run once when the state is finished before the next state is initialized.
     * This can do any cleaning up, such as stopping any started motors.
     */
    public abstract void dispose();
}

This page is out of date (unsupported feature)

The States factory class is used to keep all the implementations of the State interface in a central location. All of these implementations use BasicAbstractState or AbstractState. These factory methods are used in the StateMachineBuilder.

ftc/electronvolts/statemachine/States.java

package ftc.electronvolts.statemachine;

import java.util.Map;

import ftc.electronvolts.util.ResultReceiver;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * A factory class containing methods that return several useful states.
 *
 * To write your own State factory, make a class that extends this one and add
 * your own methods. It will inherit all these methods as well, so that when you
 * use your class, you will have access to all these methods and your own in one
 * place.
 */
public class States {
    /**
     * Creates a state machine inside a state of another state machine
     * "Yo, dawg, we heard you like states so we put a state machine inside a
     * state of another state machine so your state can act while your state
     * machine acts, dawg."
     * 
     * @param subStateToState map that links the sub states to the states in the main state machine
     * @param stateMachineBuilder the builder to add the sub-states to
     * @return the created State
     */
    public static State subStates(final Map<StateName, StateName> subStateToState, final StateMachineBuilder stateMachineBuilder) {
        for (Map.Entry<StateName, StateName> entry : subStateToState.entrySet()) {
            StateName subState = entry.getKey();
            stateMachineBuilder.addStop(subState);
        }
        final StateMachine stateMachine = stateMachineBuilder.build();
        return new BasicAbstractState() {
            private StateName nextStateName;

            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                stateMachine.act();
                for (Map.Entry<StateName, StateName> entry : subStateToState.entrySet()) {
                    StateName subState = entry.getKey();
                    StateName state = entry.getValue();
                    // if the current state is one of the ending sub-states
                    if (stateMachine.getCurrentStateName() == subState) {
                        // go to the corresponding super-state
                        nextStateName = state;
                        return true;
                    }
                }
                return false;
            }

            @Override
            public StateName getNextStateName() {
                return nextStateName;
            }
        };
    }

    /**
     * "Go back from whence you came -- and never return!"
     * 
     * A state that never returns
     *
     * @return the created state
     */
    public static State stop() {
        return new BasicAbstractState() {
            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                return false;
            }

            @Override
            public StateName getNextStateName() {
                return null;
            }
        };
    }

    /**
     * "An empty-head thinks mischief is fun, but a mindful person relishes wisdom."
     * (Proverbs 2^10-1)
     * 
     * An empty state.
     *
     * @param transitions the transitions to be considered
     * @return the created State
     */
    public static State empty(Map<StateName, EndCondition> transitions) {
        return new AbstractState(transitions) {
            @Override
            public void init() {
            }

            @Override
            public void run() {
            }

            @Override
            public void dispose() {
            }
        };
    }

    /**
     * "Stop! The cup is full!" said Tokusan.
     * 
     * "Exactly," said Master Ryutan. "You are like this cup; you are full of
     * ideas. You come and ask for teaching, but your cup is full; I can't put
     * anything in. Before I can teach you, you'll have to empty your cup."
     * 
     * An empty state.
     *
     * @param nextStateName the next state to be run
     * @return the created State
     */
    public static State empty(final StateName nextStateName) {
        return new BasicAbstractState() {
            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                return true;
            }

            @Override
            public StateName getNextStateName() {
                return nextStateName;
            }
        };
    }

    /**
     * "She set up a great loom in her palace, and set to weaving a web of
     * threads long and fine. Then she said to us: 'Young men, my suitors
     * now that the great Odysseus has perished, wait, though you are eager
     * to marry me, until I finish this web, so that my weaving will not be
     * useless and wasted.'"
     * ~Penelope, the Odyssey
     * 
     * A state used to run a thread. Useful for off-loading computer intensive
     * tasks such as image processing.
     *
     * @param nextStateName the next state to be run immediately
     * @param thread the thread to be run at the start of the state
     * @return the created State
     */
    public static State runThread(final StateName nextStateName, final Thread thread) {
        return new BasicAbstractState() {
            @Override
            public void init() {
                thread.start();
            }

            @Override
            public boolean isDone() {
                return true;
            }

            @Override
            public StateName getNextStateName() {
                return nextStateName;
            }
        };
    }

    /**
     * "The power of the Executive Branch is vested in the President of the
     * United States, who also acts as head of state and Commander-in-Chief of
     * the armed forces."
     * 
     * This is used to do branching and decision trees in the state machine
     * can be used for doing different things depending on which side of the
     * field you are on
     *
     * @param trueStateName the state to go to if the condition is true
     * @param falseStateName the state to go to if the condition is false
     * @param condition the boolean to decide which branch to go to
     * @return the created State
     */
    public static State branch(final StateName trueStateName, final StateName falseStateName, final boolean condition) {
        return new BasicAbstractState() {
            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                return true;
            }

            @Override
            public StateName getNextStateName() {
                if (condition) {
                    return trueStateName;
                } else {
                    return falseStateName;
                }
            }
        };
    }

    /**
     * "Established by Article I of the Constitution, the Legislative Branch
     * consists of the House of Representatives and the Senate, which together
     * form the United States Congress."
     * 
     * This is used to do branching and decision trees in the state machine
     * can be used for doing different things depending on which side of the
     * field you are on
     *
     * @param trueStateName the state to go to if the condition is true
     * @param falseStateName the state to go to if the condition is false
     * @param nullStateName the state to go to if the condition is null
     * @param condition the boolean to decide which branch to go to
     * @return the created State
     */
    public static State branch(final StateName trueStateName, final StateName falseStateName, final StateName nullStateName, final Boolean condition) {
        return new BasicAbstractState() {
            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                return true;
            }

            @Override
            public StateName getNextStateName() {
                if (condition == null) {
                    return nullStateName;
                } else if (condition) {
                    return trueStateName;
                } else {
                    return falseStateName;
                }
            }
        };
    }

    /**
     * "Where the Executive and Legislative branches are elected by the people,
     * members of the Judicial Branch are appointed by the President and
     * confirmed by the Senate."
     * 
     * This is used to do branching and decision trees in the state machine
     * using a result
     * acquired while it is running, and to communicate between threads
     *
     * @param trueStateName the state to go to if the receiver returns true
     * @param falseStateName the state to go to if the receiver returns false
     * @param nullStateName the state to go to if the receiver returns null
     * @param receiver the receiver that decides which branch to go to
     * @return the created State
     */
    public static State branch(final StateName trueStateName, final StateName falseStateName, final StateName nullStateName, final ResultReceiver<Boolean> receiver) {
        return new BasicAbstractState() {
            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                return receiver.isReady();
            }

            @Override
            public StateName getNextStateName() {
                if (receiver.getValue() == null) {
                    return nullStateName;
                } else if (receiver.getValue()) {
                    return trueStateName;
                } else {
                    return falseStateName;
                }
            }
        };
    }

    /**
     * Count von Count: Five bananas. Six bananas. SEVEN BANANAS!
     * [thunder strikes]
     * Count von Count: Ah, ah, ah. Now, that was silly. Wouldn't you agree, my
     * bats? Ah, ah, ah.
     * 
     * Moves to continueStateName for the first n times it is called, then moves
     * to doneStateName after that
     * 
     * @param continueStateName will be transitioned to first
     * @param doneStateName will be transitioned to after
     * @param n the number of times to return continueStateName
     * @return the created State
     */
    public static State count(final StateName continueStateName, final StateName doneStateName, final int n) {
        return new BasicAbstractState() {
            int i = 0;

            @Override
            public void init() {
                i++;
            }

            @Override
            public boolean isDone() {
                return true;
            }

            @Override
            public StateName getNextStateName() {
                return i <= n ? continueStateName : doneStateName;
            }
        };
    }
}

This page is out of date (unsupported feature)

EndConditions is a factory for the most commonly used EndCondition objects. Storing all of these end conditions in one place allows programmers to write and debug code quickly. These end conditions range from simple tasks such as reporting whether or not a certain amount of time has passed to more complicated tasks like reporting if a sensor has reached a certain value. See also: States

ftc/electronvolts/statemachine/EndConditions.java

package ftc.electronvolts.statemachine;

import java.util.Collection;

import ftc.electronvolts.util.InputExtractor;
import ftc.electronvolts.util.MatchTimer;
import ftc.electronvolts.util.ResultReceiver;
import ftc.electronvolts.util.units.Time;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * This is a factory class for the EndCondition interface.
 * It contains methods which return some useful end conditions that can be used
 * not only in the state machine, but also anywhere in your code.
 *
 * To write your own EndCondition factory, make a class that extends this one
 * and add your own methods. It will inherit all these methods as well, so that
 * when you use your class, you will have access to all these methods and your
 * own in one place.
 */
public class EndConditions {
    /**
     * An end condition that finishes after a certain amount of time
     *
     * @param durationMillis the amount of time to wait before finishing
     * @return the created EndCondition
     */
    public static EndCondition timed(final long durationMillis) {
        return new EndCondition() {
            private long endTime;

            @Override
            public void init() {
                endTime = System.currentTimeMillis() + durationMillis;
            }

            @Override
            public boolean isDone() {
                return System.currentTimeMillis() >= endTime;
            }
        };
    }

    /**
     * An end condition that finishes after a certain amount of time
     *
     * @param duration the amount of time to wait before finishing
     * @return the created EndCondition
     */
    public static EndCondition timed(Time duration) {
        return timed((long) duration.milliseconds());
    }

    /**
     * An end condition that finishes after a certain amount of time since the
     * start of the match has passed
     *
     * @param matchTimer a reference to the match timer object
     * @param millisFromMatchStart how many millis from the match start to wait
     * @return the created EndCondition
     */
    public static EndCondition matchTimed(final MatchTimer matchTimer, final long millisFromMatchStart) {

        return new EndCondition() {
            @Override
            public void init() {
            }

            @Override
            public boolean isDone() {
                return matchTimer.getElapsedTime() >= millisFromMatchStart;
            }
        };
    }
    
    /**
     * An end condition that finishes after a certain amount of time since the
     * start of the match has passed
     *
     * @param matchTimer a reference to the match timer object
     * @param timeFromMatchStart how long from the match start to wait
     * @return the created EndCondition
     */
    public static EndCondition matchTimed(MatchTimer matchTimer, Time timeFromMatchStart) {
        return matchTimed(matchTimer, (long) timeFromMatchStart.milliseconds());
    }

    /**
     * An end condition that returns the opposite of another end condition
     *
     * @param endCondition the end condition to use
     * @return the created EndCondition
     */
    public static EndCondition not(final EndCondition endCondition) {
        return new EndCondition() {
            @Override
            public void init() {
                endCondition.init();
            }

            @Override
            public boolean isDone() {
                return !endCondition.isDone();
            }
        };
    }

    /**
     * An end condition that finishes if it has been initialized a certain
     * amount of times
     *
     * @param maxCount the maximum amount of times for the end condition to be
     *            initialized
     * @return the created EndCondition
     */
    public static EndCondition count(final int maxCount) {
        return new EndCondition() {
            private int counter = 0;

            @Override
            public void init() {
                counter++;
            }

            @Override
            public boolean isDone() {
                return counter >= maxCount;
            }
        };
    }

    /**
     * An end condition that executes a certain number of loops before finishing
     *
     * @param maxCount The number of loops to allow the execution of
     * @return the created EndCondition
     */
    public static EndCondition loopCount(final int maxCount) {
        return new EndCondition() {
            private int counter;

            @Override
            public void init() {
                counter = 0;
            }

            @Override
            public boolean isDone() {
                counter++;
                return counter >= maxCount;
            }
        };
    }

    /**
     * An end condition that finishes if all of the specified end conditions are
     * true
     *
     * @param endConditions a list containing all of the end conditions
     * @return the created EndCondition
     */
    public static EndCondition all(final Collection<EndCondition> endConditions) {
        return new EndCondition() {
            @Override
            public void init() {
                for (EndCondition e : endConditions) {
                    e.init();
                }
            }

            @Override
            public boolean isDone() {
                for (EndCondition e : endConditions) {
                    if (!e.isDone()) {
                        return false;
                    }
                }
                return true;
            }
        };
    }

    /**
     * An end condition that finishes if any of the specified end conditions are
     * true
     *
     * @param endConditions a list containing all of the end conditions
     * @return the created EndCondition
     */
    public static EndCondition any(final Collection<EndCondition> endConditions) {
        return new EndCondition() {
            @Override
            public void init() {
                for (EndCondition e : endConditions) {
                    e.init();
                }
            }

            @Override
            public boolean isDone() {
                for (EndCondition e : endConditions) {
                    if (e.isDone()) {
                        return true;
                    }
                }
                return false;
            }
        };
    }

    /**
     * An end condition that never finishes
     *
     * @return the created EndCondition
     */
    public static EndCondition never() {
        return new EndCondition() {
            @Override
            public void init() {

            }

            @Override
            public boolean isDone() {
                return false;
            }
        };
    }

    /**
     * An end condition that only allows the state to execute one loop
     *
     * @return the created EndCondition
     */
    public static EndCondition now() {
        return new EndCondition() {
            @Override
            public void init() {

            }

            @Override
            public boolean isDone() {
                return true;
            }
        };
    }

    // The following few end conditions use input extractors
    // They may or may not be useful to most people since it would usually be
    // easier to write your
    // own end condition instead of using these

    /**
     * This could be used for an end condition that uses the joystick
     *
     * @param inputExtractor an extracted boolean
     * @return the created EndCondition
     */
    public static EndCondition inputExtractor(final InputExtractor<Boolean> inputExtractor) {
        return new EndCondition() {
            @Override
            public void init() {

            }

            @Override
            public boolean isDone() {
                return inputExtractor.getValue();
            }
        };
    }

    /**
     * compares the values of two input extractors
     *
     * @param inputExtractorA the first input extractor
     * @param inputExtractorB the second input extractor
     * @param inclusive whether or not the values being equal satisfies the
     *            condition
     * @return the created EndCondition
     */
    public static EndCondition aGreaterThanB(final InputExtractor<Double> inputExtractorA, final InputExtractor<Double> inputExtractorB, final boolean inclusive) {
        return new EndCondition() {
            @Override
            public void init() {

            }

            @Override
            public boolean isDone() {
                if (inclusive) {
                    return inputExtractorA.getValue() >= inputExtractorB.getValue();
                } else {
                    return inputExtractorA.getValue() > inputExtractorB.getValue();
                }
            }
        };
    }

    /**
     * @param inputExtractor the input extractor
     * @param target the target value
     * @param inclusive whether or not the values being equal satisfies the
     *            condition
     * @return the created EndCondition
     */
    public static EndCondition valueGreater(final InputExtractor<Double> inputExtractor, final double target, final boolean inclusive) {
        return new EndCondition() {
            @Override
            public void init() {

            }

            @Override
            public boolean isDone() {
                if (inclusive) {
                    return inputExtractor.getValue() >= target;
                } else {
                    return inputExtractor.getValue() > target;
                }
            }
        };
    }

    /**
     * @param inputExtractor the input extractor
     * @param target the target value
     * @param inclusive whether or not the values being equal satisfies the
     *            condition
     * @return the created EndCondition
     */
    public static EndCondition valueLess(final InputExtractor<Double> inputExtractor, final double target, final boolean inclusive) {
        return new EndCondition() {
            @Override
            public void init() {

            }

            @Override
            public boolean isDone() {
                if (inclusive) {
                    return inputExtractor.getValue() <= target;
                } else {
                    return inputExtractor.getValue() < target;
                }
            }
        };
    }

    /**
     * @param inputExtractor the input extractor
     * @param min the lower edge of the target range
     * @param max the upper edge of the target range
     * @param inclusive whether or not the value being equal to the min or max
     *            satisfies the condition
     *            when min = max, this should be true.
     * @return the created EndCondition
     */
    public static EndCondition valueBetween(final InputExtractor<Double> inputExtractor, final double min, final double max, final boolean inclusive) {
        return new EndCondition() {
            @Override
            public void init() {

            }

            @Override
            public boolean isDone() {
                double value = inputExtractor.getValue();
                if (inclusive) {
                    return min <= value && value <= max;
                } else {
                    return min < value && value < max;
                }
            }
        };
    }

    /**
     * @param inputExtractor the input extractor
     * @param target the target value
     * @param tolerance the tolerance +/- the value to be accepted as meeting
     *            the condition
     * @param inclusive whether or not the value being on the edge of the
     *            tolerance satisfies the condition.
     *            when the tolerance is 0, this should be true.
     * @return the created EndCondition
     */
    public static EndCondition valueCloseTo(InputExtractor<Double> inputExtractor, double target, double tolerance, boolean inclusive) {
        return valueBetween(inputExtractor, target - tolerance, target + tolerance, inclusive);
    }
    
    /**
     * An EndCondition that waits for a ResultReceiver to have a result
     * 
     * @param receiver the ResultReceiver of any type
     * @return the created EndCondition
     */
    public static EndCondition receiverReady(final ResultReceiver<?> receiver) {
        return new EndCondition() {
            @Override
            public void init() {
                
            }
            
            @Override
            public boolean isDone() {
                return receiver.isReady();
            }
        };
    }
}

StateMachineBuilder is responsible for setting up the linkage between the states. It stores a map of the names of the states and the state itself. It also includes convenience methods to easily add pre-defined States to the state machine, reducing the complexity of adding states. When all the states are done being added, the build method is called, which returns a StateMachine object.

ftc/electronvolts/statemachine/StateMachineBuilder.java

package ftc.electronvolts.statemachine;

import java.util.HashMap;
import java.util.Map;

import ftc.electronvolts.util.MatchTimer;
import ftc.electronvolts.util.ResultReceiver;
import ftc.electronvolts.util.units.Time;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * The state machine builder simplifies the creation of the state machine. The
 * builder requires an enum with values for each state. See the wiki for an
 * example of how to use it.
 *
 * To write your own StateMachine builder, make a class that extends this one
 * and add your own convenience methods such as addDrive or addServoTurn. It
 * will inherit all these methods as well, so that when you use your class, you
 * will have access to all these methods and your own in one place.
 */
public class StateMachineBuilder {
    // the map the links a state's name to the state
    private Map<StateName, State> stateMap = new HashMap<>();
    private final StateName firstStateName;

    /**
     * A constructor for the builder. The State machine must have a state to
     * start with
     *
     * @param firstStateName the first state to be executed in order
     */
    public StateMachineBuilder(StateName firstStateName) {
        this.firstStateName = firstStateName;
    }

    /**
     * Create a new transition
     *
     * @param nextStateName the name of the enum for the next state
     * @param endCondition the end condition for the next state
     * @return a map containing the one transition
     */
    public Map<StateName, EndCondition> t(StateName nextStateName, EndCondition endCondition) {
        return StateMap.of(nextStateName, endCondition);
    }

    /**
     * Create a new transition with a timed end condition
     *
     * @param nextStateName the enum value associated with the next state
     * @param durationMillis the amount of time to wait before advancing to the
     *            next state
     * @return a map containing the one transition
     */
    public Map<StateName, EndCondition> t(StateName nextStateName, long durationMillis) {
        return t(nextStateName, EndConditions.timed(durationMillis));
    }

    /**
     * Create a new transition with a timed end condition
     *
     * @param nextStateName the enum value associated with the next state
     * @param duration the amount of time to wait before advancing to the next
     *            state
     * @return a map containing the one transition
     */
    public Map<StateName, EndCondition> t(StateName nextStateName, Time duration) {
        return t(nextStateName, EndConditions.timed(duration));
    }

    /**
     * Add a state to the state machine
     *
     * @param state the state to be added
     */
    public void add(StateName stateName, State state) {
        stateMap.put(stateName, state);
    }

    /**
     * Add an empty state that waits a certain duration
     *
     * @param stateName the enum value to be associated with the wait state
     * @param nextStateName the name of the next state
     * @param durationMillis the length of time to wait in millis
     */
    public void addWait(StateName stateName, StateName nextStateName, long durationMillis) {
        add(stateName, States.empty(t(nextStateName, EndConditions.timed(durationMillis))));
    }

    /**
     * Add an empty state that waits a certain duration
     *
     * @param stateName the enum value to be associated with the wait state
     * @param nextStateName the name of the next state
     * @param duration the length of time to wait
     */
    public void addWait(StateName stateName, StateName nextStateName, Time duration) {
        add(stateName, States.empty(t(nextStateName, EndConditions.timed(duration))));
    }

    /**
     * Add an empty state that waits until a certain amount of time has passed
     * since the beginning of the match
     *
     * @param stateName the enum value to be associated with the wait state
     * @param nextStateName the name of the next state
     * @param matchTimer a reference to the match timer object
     * @param durationMillis how many millis from the match start to wait
     * @see MatchTimer
     */
    public void addWait(StateName stateName, StateName nextStateName, MatchTimer matchTimer, long durationMillis) {
        add(stateName, States.empty(t(nextStateName, EndConditions.matchTimed(matchTimer, durationMillis))));
    }

    /**
     * Add an empty state that waits until a certain amount of time has passed
     * since the beginning of the match
     *
     * @param stateName the enum value to be associated with the wait state
     * @param nextStateName the name of the next state
     * @param matchTimer a reference to the match timer object
     * @param duration how much time from the match start to wait
     * @see MatchTimer
     */
    public void addWait(StateName stateName, StateName nextStateName, MatchTimer matchTimer, Time duration) {
        add(stateName, States.empty(t(nextStateName, EndConditions.matchTimed(matchTimer, duration))));
    }

    /**
     * @param stateName the enum value to be associated with the wait state
     * @param trueStateName the state to go to if the condition is true
     * @param falseStateName the state to go to if the condition is false
     * @param condition the boolean to decide which branch to go to
     */
    public void addBranch(StateName stateName, StateName trueStateName, StateName falseStateName, boolean condition) {
        add(stateName, States.branch(trueStateName, falseStateName, condition));
    }

    /**
     * @param stateName the enum value to be associated with the wait state
     * @param trueStateName the state to go to if the condition is true
     * @param falseStateName the state to go to if the condition is false
     * @param nullStateName the state to go to if the condition is null
     * @param condition the boolean to decide which branch to go to
     */
    public void addBranch(StateName stateName, StateName trueStateName, StateName falseStateName, StateName nullStateName, Boolean condition) {
        add(stateName, States.branch(trueStateName, falseStateName, nullStateName, condition));
    }

    /**
     * @param stateName the enum value to be associated with the wait state
     * @param trueStateName the state to go to if the receiver returns true
     * @param falseStateName the state to go to if the receiver returns false
     * @param nullStateName the state to go to if the receiver returns null
     * @param receiver the receiver that decides which branch to go to
     */
    public void addBranch(StateName stateName, StateName trueStateName, StateName falseStateName, StateName nullStateName, ResultReceiver<Boolean> receiver) {
        add(stateName, States.branch(trueStateName, falseStateName, nullStateName, receiver));
    }

    /**
     * Adds an empty state that does nothing and moves to the next state
     * 
     * @param stateName the name of the state
     * @param nextStateName the name of the state to go to next
     */
    public void addEmpty(StateName stateName, StateName nextStateName) {
        add(stateName, States.empty(nextStateName));
    }

    public void addEmpty(StateName stateName, Map<StateName, EndCondition> transitions) {
        add(stateName, States.empty(transitions));
    }

    /**
     * Adds a stop state to the stateMap
     *
     * @param stateName name of the stop state
     */
    public void addStop(StateName stateName) {
        add(stateName, States.stop());
    }

    /**
     * A state used to run a thread. Useful for off-loading computer intensive
     * tasks such as image processing.
     *
     * @param stateName the name of the state
     * @param nextStateName the next state to be transitioned to immediately
     * @param thread the thread to be run at the start of the state
     */
    public void addThread(StateName stateName, StateName nextStateName, Thread thread) {
        add(stateName, States.runThread(nextStateName, thread));
    }

    /**
     * Add a state that moves to continueStateName for the first n times it is
     * called, then moves to doneStateName after that
     * 
     * @param stateName the name of the state
     * @param continueStateName will be transitioned to first
     * @param doneStateName will be transitioned to after
     * @param n the number of times to return continueStateName
     */
    public void addCount(StateName stateName, StateName continueStateName, StateName doneStateName, int n) {
        add(stateName, States.count(continueStateName, doneStateName, n));
    }

    /**
     * Build the state machine with the added states
     *
     * @return the created state machine
     * @see StateMachine
     */
    public StateMachine build() {
        return new StateMachine(stateMap, firstStateName);
    }

    /**
     * Build the state machine with the added states
     *
     * @param firstStateName the state to start at
     * @return the created state machine
     * @see StateMachine
     */

    public StateMachine build(StateName firstStateName) {
        return new StateMachine(stateMap, firstStateName);
    }
}

StateMachine is responsible for managing the linkage of the states while running. It is usually created from a StateMachineBuilder. To run it, call the act() method each time through the loop. This will have the current State act and then determine the next State based on what the current State returned. For debugging, you can call getCurrentStateName() each time in the loop and store it to a file or display it on the telemetry.

ftc/electronvolts/statemachine/StateMachine.java

package ftc.electronvolts.statemachine;

import java.util.Map;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * This class uses the stateMap made by the stateMachineBuilder to run the
 * states and perform the transitions between them
 */
public class StateMachine {
    // This links the names of each state to the actual state
    private final Map<StateName, State> stateMap;
    
    // the current active state
    private State currentState;
    
    // the name of the current state
    private StateName currentStateName;

    /**
     * @param stateMap the state machine structure
     * @param firstStateName the name of the state to start with
     */
    public StateMachine(Map<StateName, State> stateMap, StateName firstStateName) {
        this.stateMap = stateMap;
        currentStateName = firstStateName;
        currentState = stateMap.get(firstStateName);
        // if the stateMap does not have the firstStateName as a key, there is
        // no way to fix it here
    }

    /**
     * call this each time through the loop
     * It runs the current state, then looks up the next state based on the name
     * given by the current state.
     */
    public void act() {
        // tell the current state to act and return the next state
        StateName nextStateName = currentState.act();
        // if the next state name returned is null or the same as this state,
        // stay in the current state
        if (nextStateName != null && nextStateName != currentStateName) {
            // if the state requested exists
            if (stateMap.containsKey(nextStateName)) {
                // update the current state
                currentStateName = nextStateName;
                currentState = stateMap.get(currentStateName);
            }
        }
    }

    /**
     * used for telemetry purposes
     *
     * @return the name of the current state
     */
    public StateName getCurrentStateName() {
        return currentStateName;
    }
}

DeadZone is an interface that tells if a given value is inside the dead-zone. It is not very heavily used in the framework.

ftc/electronvolts/util/DeadZone.java

package ftc.electronvolts.util;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * A class that defines a deadzone by returning a boolean as a function of a
 * double
 */
public interface DeadZone {
    boolean isInside(double value);
}

DeadZones is a factory class for the DeadZone interface. It has methods that return DeadZones for min and max, center and delta, and combinations of other DeadZone objects.

ftc/electronvolts/util/DeadZones.java

package ftc.electronvolts.util;

import java.util.Collection;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * A factory class for DeadZone
 */
public class DeadZones {
    /**
     * @param deadZones the list of DeadZones to be combined
     * @return the composite DeadZone
     */
    public static DeadZone any(final Collection<DeadZone> deadZones) {
        return new DeadZone() {
            @Override
            public boolean isInside(double value) {
                for (DeadZone deadZone : deadZones) {
                    if (deadZone.isInside(value)) {
                        return true;
                    }
                }
                return false;
            }
        };
    }

    /**
     * @param deadZones the list of DeadZones to be combined
     * @return the composite DeadZone
     */
    public static DeadZone all(final Collection<DeadZone> deadZones) {
        return new DeadZone() {
            @Override
            public boolean isInside(double value) {
                for (DeadZone deadZone : deadZones) {
                    if (!deadZone.isInside(value)) {
                        return false;
                    }
                }
                return true;
            }
        };
    }

    /**
     * inverts a DeadZone
     *
     * @param deadZone the DeadZone to invert
     * @return the resulting DeadZone
     */
    public static DeadZone not(final DeadZone deadZone) {
        return new DeadZone() {
            @Override
            public boolean isInside(double value) {
                return !deadZone.isInside(value);
            }
        };
    }

    /**
     * a DeadZone that is empty
     *
     * @return the DeadZone
     */
    public static DeadZone noDeadZone() {
        return new DeadZone() {
            @Override
            public boolean isInside(double value) {
                return false;
            }
        };
    }

    /**
     * A DeadZone between min and max
     *
     * @param min the lower edge of the DeadZone
     * @param max the upper edge of the DeadZone
     * @return the DeadZone
     */
    public static DeadZone minMaxDeadzone(final double min, final double max) {
        return new DeadZone() {
            @Override
            public boolean isInside(double value) {
                return (value >= min && value <= max);
            }
        };
    }

    /**
     * deadzone that ranges from center-delta to center+delta
     * center is usually zero so that the deadzone is +/-delta
     *
     * @param center the center of the deadzone
     * @param delta the delta on either side of the deadzone
     * @return the DeadZone
     */
    public static DeadZone deltaDeadZone(double center, double delta) {
        double min = center - delta;
        double max = center + delta;
        return minMaxDeadzone(min, max);
    }
}

ControlLoop defines an interface for control loops such as a PID controller or a proportional controller. The computeCorrection function takes a target value, or setPoint, and an actual value, or input. It computes the power necessary for a motor or other output to bring the input closer to the setPoint.

The initialize() function is meant to reset the persistent (stored between loop cycles) variables. It should be called when the controller has been off for a while.

ftc/electronvolts/util/ControlLoop.java

package ftc.electronvolts.util;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * An interface that allows for any type of control loop to be used
 * @see PIDController
 */
public interface ControlLoop {
    double computeCorrection(double setPoint, double input);
    void initialize();
}

PIDController implements ControlLoop. It computes the correction on an output based on an input and 4 constants: P, I, D, and maximum output. These 4 constants are given when PIDController is created.

The main function is computeCorrection. It takes the target position (setPoint) and the measured actual position (input) as parameters. It finds the delta time (timeChange), and from that calculates the change in the integral-term (iTerm) and the change in input per time (dInput). Then it calculates output:

output = pGain * error + iTerm - dGain * dInput;

• pGain * error is the Proportional term
• iTerm is the Integral term that is accumulated over time
• dGain * dInput is the Derivative term that is calculated from the delta input

The initialize() function should be called when the controller has been inactive for a while.

ftc/electronvolts/util/PIDController.java

package ftc.electronvolts.util;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * A PID controller to use for controlling motors and other outputs
 */
public class PIDController implements ControlLoop {
    private final double pGain, iGain, dGain, maxOutput;
    private final boolean hasIOrDComponent;
    private double iTerm = 0, output = 0;
    private double input = 0, lastInput = 0;
    private long lastTime = -1;

    /**
     * create a new PID controller
     *
     * @param pGain p constant (cannot be negative)
     * @param iGain i constant (cannot be negative)
     * @param dGain d constant (cannot be negative)
     * @param maxOutput the max value of the output and iTerm (cannot be
     *            negative)
     */
    public PIDController(double pGain, double iGain, double dGain, double maxOutput) {
        if (pGain < 0) {
            throw new IllegalArgumentException("Illegal pGain constant \"" + pGain + "\". PID constants cannot be negative.");
        }
        if (iGain < 0) {
            throw new IllegalArgumentException("Illegal iGain constant \"" + iGain + "\". PID constants cannot be negative.");
        }
        if (dGain < 0) {
            throw new IllegalArgumentException("Illegal dGain constant \"" + dGain + "\". PID constants cannot be negative.");
        }
        if (maxOutput < 0) {
            throw new IllegalArgumentException("Illegal maxOutput constant \"" + maxOutput + "\". PID constants cannot be negative.");
        }

        this.pGain = pGain;
        this.iGain = iGain;
        this.dGain = dGain;
        this.maxOutput = maxOutput;

        hasIOrDComponent = (iGain != 0 || dGain != 0);
    }

    /**
     * @param setPoint the target value
     * @param input the actual value
     * @return the output of the PID
     */
    @Override
    public double computeCorrection(double setPoint, double input) {
        long now = System.currentTimeMillis();
        if (lastTime < 0) {
            lastTime = now;
        }

        //time passed since last cycle
        double dTime = now - lastTime;
        if (dTime > 0 || !hasIOrDComponent) {
            this.input = input;

            // Compute all the working error variables
            double error = setPoint - input;
            iTerm += iGain * error * dTime;
            iTerm = Utility.mirrorLimit(iTerm, maxOutput);

            // compute dInput instead of dError to avoid spikes
            double dInput = 0;
            if (dTime > 0) dInput = (input - lastInput) / dTime;

            // Compute PID Output
            output = pGain * error + iTerm - dGain * dInput;
            output = Utility.mirrorLimit(output, maxOutput);

            // Remember some variables for next time
            lastInput = input;
            lastTime = now;
        }

        return output;
    }

    /**
     * Reset the PIDController to its initial state by resetting the iTerm and
     * the lastTime
     */
    @Override
    public void initialize() {
        lastInput = input;
        iTerm = 0;
        lastTime = -1;
        if (hasIOrDComponent) {
            output = 0;
        }
    }
}

ProportionalController implements ControlLoop. It provides a curve that is shaped like the following graph (parameters new ProportionalController(0.2, 0.05, 0.1, 0.5))

The parameters are as follows:

• gain -- the slope of the line
• deadzone -- the portion of the line where y=0
• minOutput -- the height of the small flat parts on either side of the deadzone
• maxOutput -- the height of the large flat parts on the edges of the graph

ftc/electronvolts/util/ProportionalController.java

package ftc.electronvolts.util;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * A proportional controller to use for controlling motors and other outputs
 */
public class ProportionalController implements ControlLoop {
    private final double gain, maxOutput, deadzone, minOutput;

    public ProportionalController(double gain, double deadzone, double minOutput, double maxOutput) {
        if (gain < 0) {
            throw new IllegalArgumentException("Illegal gain constant \"" + gain + "\". constants cannot be negative.");
        }
        if (maxOutput < 0) {
            throw new IllegalArgumentException("Illegal maxOutput constant \"" + maxOutput + "\". constants cannot be negative.");
        }
        if (deadzone < 0) {
            throw new IllegalArgumentException("Illegal deadzone constant \"" + deadzone + "\". constants cannot be negative.");
        }
        if (minOutput < 0) {
            throw new IllegalArgumentException("Illegal minOutput constant \"" + minOutput + "\". constants cannot be negative.");
        }
        if (minOutput > maxOutput) {
            throw new IllegalArgumentException("minOutput (" + minOutput + ") cannot be grater than maxOutput (" + maxOutput + ")");
        }
        if (deadzone > minOutput) {
            throw new IllegalArgumentException("deadzone (" + deadzone + ") cannot be grater than minOutput (" + minOutput + ")");
        }

        this.gain = gain;
        this.maxOutput = maxOutput;
        this.deadzone = deadzone;
        this.minOutput = minOutput;
    }

    @Override
    public double computeCorrection(double setPoint, double input) {
        double correction = gain * (setPoint - input);

        if (Math.abs(correction) <= deadzone) {
            //return 0 if the error is in the deadzone
            return 0;
        } else if (Math.abs(correction) < minOutput) {
            //return the minimum if it is below
            return Math.signum(correction) * minOutput;
        } else if (Math.abs(correction) > maxOutput) {
            //cap the correction at +/- maxOutput
            return Math.signum(correction) * maxOutput;
        } else {
            return correction;
        }
    }

    @Override
    public void initialize() {
        //not needed for a proportional controller since there are no persistent state variables
    }

}

In EVLib there are 2 types of motors. Motor, which has no encoder, and MotorEnc, which has an encoder. A MotorEnc extends Motor, which means that you can use a motor with an encoder as a motor without an encoder. To create instances of these, see the Creating Motors page.

The interface for Motor has only a few methods, and the only way to control it is by setting the power.

ftc/evlib/hardware/motors/Motor.java

package ftc.evlib.hardware.motors;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 9/11/16
 * 
 * Wrapper class for the DcMotor.
 * This represents the functions a motor without an encoder can do.
 *
 * @see MotorEnc
 * @see Motors
 */
public interface Motor {
    /**
     * The different modes the motor can be in
     */
    enum Mode {
        POWER, //directly control the power
        SPEED, //enable a feedback loop to correct speed (requires encoders)
        POSITION //turn the motor to a certain encoder position (requires encoders)
    }

    /**
     * Control the motor's raw voltage
     *
     * @param power value to set the power to
     */
    void setPower(double power);

    /**
     * Tells the mode the motor is in which is determined by the last command to the motor.
     *
     * @return the current mode
     */
    Mode getMode();

    /**
     * Sends motor commands to the motor controller
     */
    void update();
}

The MotorEnc interface has all the methods from Motor, plus the ones for the encoder-related functions.

ftc/evlib/hardware/motors/MotorEnc.java

package ftc.evlib.hardware.motors;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 9/12/16
 * 
 * Wrapper class for DcMotor if the motor has an encoder
 * This interface has all the non-encoder methods from the Motor interface plus the ones shown here.
 * It can be passed in where a non-encoder Motor interface is needed.
 *
 * @see Motor
 * @see Motors
 */
public interface MotorEnc extends Motor {
    /**
     * A PID on the motor controller uses the encoder to regulate the speed of the motor.
     *
     * @param speed value to set the speed to
     */
    void setSpeed(double speed);

    /**
     * A PID on the motor controller uses the encoder to turn the motor to any encoder position.
     *
     * @param encoderTarget      position in encoder ticks to rotate to
     * @param maxCorrectionPower the max power to run the motor at when turning to the position
     */
    void setPosition(int encoderTarget, double maxCorrectionPower);

    /**
     * Set the encoder zero point to the current encoder value
     */
    void resetEncoder();

    /**
     * Get the encoder position relative to the zero value
     *
     * @return the encoder position
     */
    int getEncoderPosition();

}

Before you read this, it is helpful to read about the Motor and MotorEnc interfaces.

To get a DcMotor from the hardwareMap and wrap it with the Motor interface, you can do the following:

Stoppers stoppers = new Stoppers(); //pass this in each time you create a motor
boolean reversed = false;
boolean brake = true;

DcMotor leftDcMotor = hardwareMap.dcMotor.get("leftMotor")
Motor leftMotor = Motors.withoutEncoder(leftDcMotor, reversed, brake, stoppers);

or you can combine them into 1 line:

Motor leftMotor = Motors.withoutEncoder(hardwareMap, "leftMotor", false, true, stoppers);

You can also create a Motor from a Continuous servo:

boolean reversed = false;
Motor arm = Motors.continuousServo(hardwareMap.crservo.get("servoArm"), reversed);

To combine two motors:

Motor combined = Motors.combinedWithoutEncoder(leftMotor, arm);

To combine more than 2 motors:

Motor combined = Motors.combinedWithoutEncoder(
        ImmutableList.of(motor1, motor2, motor3, ...)
);

Here are all the methods for creating motors:

This example uses guava to initialize lists, which we have left for alternative methods.

ftc/evlib/hardware/motors/Motors.java

package ftc.evlib.hardware.motors;

import com.google.common.collect.ImmutableList;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;

import java.util.List;

import ftc.electronvolts.util.Function;
import ftc.electronvolts.util.Utility;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 9/11/16
 *
 * Factory class for creating Motor wrapper classes.
 * Has methods for all the combinations of with/without encoders and forward/reversed.
 *
 * @see Motor
 * @see MotorEnc
 */
public class Motors {
    /**
     * combine two motors with encoders into one motor
     *
     * @param motorEnc1 the first motor (with encoder)
     * @param motorEnc2 the second motor (with encoder)
     * @return the motor that controls both (with encoder support)
     */
    public static MotorEnc combinedWithEncoder(MotorEnc motorEnc1, MotorEnc motorEnc2) {
        return combinedWithEncoder(ImmutableList.of(motorEnc1, motorEnc2));
    }

    /**
     * combines any number of motors with encoders into one
     *
     * @param motorEncs the list of motors to combine (all must have encoders)
     * @return the motor that controls all of them (with encoder support)
     */
    public static MotorEnc combinedWithEncoder(final List<MotorEnc> motorEncs) {
        return new MotorEnc() {
            @Override
            public void setSpeed(double speed) {
                for (MotorEnc motorEnc : motorEncs)
                    motorEnc.setSpeed(speed);
            }

            @Override
            public void setPosition(int encoderTarget, double maxCorrectionPower) {
                for (MotorEnc motorEnc : motorEncs)
                    motorEnc.setPosition(encoderTarget, maxCorrectionPower);
            }

            @Override
            public void resetEncoder() {
                for (MotorEnc motorEnc : motorEncs) motorEnc.resetEncoder();
            }

            @Override
            public int getEncoderPosition() {
                int total = 0;
                for (MotorEnc motorEnc : motorEncs) total += motorEnc.getEncoderPosition();
                if (motorEncs.size() == 0) {
                    return 0;
                } else {
                    return total / motorEncs.size();
                }
            }

            @Override
            public void setPower(double power) {
                for (MotorEnc motorEnc : motorEncs) motorEnc.setPower(power);
            }

            @Override
            public Mode getMode() {
                if (motorEncs.size() == 0) {
                    return Mode.POWER;
                } else {
                    return motorEncs.get(0).getMode();
                }
            }

            @Override
            public void update() {
                for (MotorEnc motorEnc : motorEncs) motorEnc.update();
            }
        };
    }

    /**
     * combine two motors with or without encoders into one motor
     *
     * @param motor1 the first motor
     * @param motor2 the second motor
     * @return the motor that controls both (without encoder support)
     */
    public static Motor combinedWithoutEncoder(Motor motor1, Motor motor2) {
        return combinedWithoutEncoder(ImmutableList.of(motor1, motor2));
    }

    /**
     * combines any number of motors with or without encoders into one
     *
     * @param motors the list of motors to combine
     * @return the motor that controls all of them (without encoder support)
     */
    public static Motor combinedWithoutEncoder(final List<Motor> motors) {
        return new Motor() {
            @Override
            public void setPower(double power) {
                for (Motor motor : motors) motor.setPower(power);
            }

            @Override
            public Mode getMode() {
                return Mode.POWER;
            }

            @Override
            public void update() {
                for (Motor motor : motors) motor.update();
            }
        };
    }

    /**
     * Initialize a dcMotor by setting parameters and checking that they were set properly
     *
     * @param dcMotor  the motor to initialize
     * @param reversed whether or not the motor direction should be reversed
     * @param brake    whether to brake or float when stopping
     * @param runMode  what mode to start the motor in
     */
    private static void dcMotorInit(DcMotor dcMotor, boolean reversed, boolean brake, DcMotor.RunMode runMode) {
        //reset the encoder position to zero
        do {
            dcMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        } while (dcMotor.getMode() != DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        //determine the motor's direction as a Direction object
        DcMotor.Direction direction;
        if (reversed) {
            direction = DcMotorSimple.Direction.REVERSE;
        } else {
            direction = DcMotorSimple.Direction.FORWARD;
        }

        //set the motor's direction
        do {
            dcMotor.setDirection(direction);
        } while (dcMotor.getDirection() != direction);

        //set the motor's mode
        do {
            dcMotor.setMode(runMode);
        } while (dcMotor.getMode() != runMode);

        //determine the ZeroPowerBehavior
        DcMotor.ZeroPowerBehavior zeroPowerBehavior;
        if (brake) {
            zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE;
        } else {
            zeroPowerBehavior = DcMotor.ZeroPowerBehavior.FLOAT;
        }

        //set the motor's ZeroPowerBehavior
        do {
            dcMotor.setZeroPowerBehavior(zeroPowerBehavior);
        } while (dcMotor.getZeroPowerBehavior() != zeroPowerBehavior);
    }

    /**
     * Create a Motor from the hardware map
     *
     * @param hardwareMap              the hardwareMap from the opmode
     * @param dcMotorName              the name of the DcMotor in the hardwareMap
     * @param reversed                 true if the motor's direction should be reversed
     * @param brake                    true if the motor should brake when stopped
     * @param stoppers                 the Stoppers object to add the motor to
     * @return the created MotorEnc
     */
    public static Motor withoutEncoder(HardwareMap hardwareMap, String dcMotorName, boolean reversed, boolean brake, Stoppers stoppers) {
        return withoutEncoder(hardwareMap.dcMotor.get(dcMotorName), reversed, brake, stoppers);
    }

    /**
     * Create a Motor from a DcMotor
     *
     * @param dcMotor                  the DcMotor to be wrapped
     * @param reversed                 true if the motor's direction should be reversed
     * @param brake                    true if the motor should brake when stopped
     * @param stoppers                 the Stoppers object to add the motor to
     * @return the created MotorEnc
     */
    public static Motor withoutEncoder(final DcMotor dcMotor, boolean reversed, boolean brake, Stoppers stoppers) {
        //initialize the motor with no encoder
        dcMotorInit(dcMotor, reversed, brake, DcMotor.RunMode.RUN_WITHOUT_ENCODER);

        stoppers.add(new Stopper() {
            @Override
            public void stop() {
                do {
                    dcMotor.setPower(0);
                } while (dcMotor.getPower() != 0);
            }
        });

        return new Motor() {
            private double power = 0;

            @Override
            public void setPower(double power) {
                this.power = power;
            }

            @Override
            public Mode getMode() {
                return Mode.POWER;
            }

            @Override
            public void update() {
                dcMotor.setPower(Utility.motorLimit(power));
            }
        };
    }

    /**
     * Convert a Motor.MotorMode to a DcMotor.RunMode
     *
     * @param mode the mode to convert
     * @return the corresponding DcMotor.RunMode
     */
    public static DcMotor.RunMode motorModeToDcMotorRunMode(Motor.Mode mode) {
        switch (mode) {
            case POWER:
                return DcMotor.RunMode.RUN_WITHOUT_ENCODER;
            case SPEED:
                return DcMotor.RunMode.RUN_USING_ENCODER;
            case POSITION:
                return DcMotor.RunMode.RUN_TO_POSITION;
            default:
                return null;
        }
    }

    /**
     * Convert a DcMotor.RunMode to a Motor.Mode
     *
     * @param runMode the mode to convert
     * @return the corresponding Motor.Mode
     */
    public static Motor.Mode dcMotorRunModeToMotorMode(DcMotor.RunMode runMode) {
        switch (runMode) {
            case RUN_WITHOUT_ENCODER:
                return Motor.Mode.POWER;
            case RUN_USING_ENCODER:
                return Motor.Mode.SPEED;
            case RUN_TO_POSITION:
                return Motor.Mode.POSITION;
            default:
                return null;
        }
    }

    /**
     * Create a MotorEnc from the hardware map
     *
     * @param hardwareMap              the hardwareMap from the opmode
     * @param dcMotorName              the name of the DcMotor in the hardwareMap
     * @param maxEncoderTicksPerSecond the encoder ticks per second at max power
     * @param reversed                 true if the motor's direction should be reversed
     * @param brake                    true if the motor should brake when stopped
     * @param stoppers                 the Stoppers object to add the motor to
     * @return the created MotorEnc
     */
    public static MotorEnc withEncoder(HardwareMap hardwareMap, String dcMotorName, int maxEncoderTicksPerSecond, boolean reversed, boolean brake, Stoppers stoppers) {
        return withEncoder(hardwareMap.dcMotor.get(dcMotorName), maxEncoderTicksPerSecond, reversed, brake, stoppers);
    }

    /**
     * Create a MotorEnc from a DcMotor
     *
     * @param dcMotor                  the DcMotor to be wrapped
     * @param maxEncoderTicksPerSecond the encoder ticks per second at max power
     * @param reversed                 true if the motor's direction should be reversed
     * @param brake                    true if the motor should brake when stopped
     * @param stoppers                 the Stoppers object to add the motor to
     * @return the created MotorEnc
     */
    public static MotorEnc withEncoder(final DcMotor dcMotor, int maxEncoderTicksPerSecond, boolean reversed, boolean brake, Stoppers stoppers) {
        final Motor.Mode initMode = Motor.Mode.SPEED;
        dcMotorInit(dcMotor, reversed, brake, motorModeToDcMotorRunMode(initMode)); //start with speed mode

        do {
            dcMotor.setMaxSpeed(maxEncoderTicksPerSecond);
        } while (dcMotor.getMaxSpeed() != maxEncoderTicksPerSecond);

        stoppers.add(new Stopper() {
            @Override
            public void stop() {
                do {
                    dcMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
                } while (dcMotor.getMode() != DcMotor.RunMode.RUN_WITHOUT_ENCODER);
                do {
                    dcMotor.setPower(0);
                } while (dcMotor.getPower() != 0);
            }
        });

        return new MotorEnc() {
            private int encoderZero = 0, encoderPosition = 0;
            private Mode mode = initMode, lastMode = initMode;
            private double power = 0;
            private int encoderTarget = 0;

            @Override
            public void setPower(double power) {
                mode = Mode.POWER;
                this.power = power;
            }

            @Override
            public void setSpeed(double speed) {
                mode = Mode.SPEED;
                power = speed;
            }

            @Override
            public void setPosition(int encoderTarget, double maxCorrectionPower) {
                mode = Mode.POSITION;
                this.encoderTarget = encoderTarget;
                power = maxCorrectionPower;
            }

            @Override
            public void resetEncoder() {
                encoderZero = encoderPosition;
            }

            @Override
            public int getEncoderPosition() {
                return encoderPosition - encoderZero;
            }

            @Override
            public Mode getMode() {
                return mode;
            }

            @Override
            public void update() {
                encoderPosition = dcMotor.getCurrentPosition();

                if (mode != lastMode) {
                    dcMotor.setMode(motorModeToDcMotorRunMode(mode));
                    lastMode = dcMotorRunModeToMotorMode(dcMotor.getMode());
                }

                switch (mode) {
                    case POWER:
                        break;
                    case SPEED:
                        break;
                    case POSITION:
                        dcMotor.setTargetPosition(encoderTarget);
                        break;
                }
                dcMotor.setPower(Utility.motorLimit(power));
            }
        };
    }

    /**
     * Wraps a continuous rotation servo as a normal motor
     *
     * @param crServo  the servo to be wrapped as a motor
     * @param reversed true if the servo should be reversed
     * @return the Motor wrapper class
     */
    public static Motor continuousServo(final CRServo crServo, boolean reversed) {
        DcMotorSimple.Direction direction;
        if (reversed) {
            direction = DcMotorSimple.Direction.REVERSE;
        } else {
            direction = DcMotorSimple.Direction.FORWARD;
        }

        do {
            crServo.setDirection(direction);
        } while (crServo.getDirection() != direction);

        return new Motor() {
            private double power = 0;

            @Override
            public void setPower(double power) {
                this.power = power;
            }

            @Override
            public Mode getMode() {
                return Mode.POWER;
            }

            @Override
            public void update() {
                crServo.setPower(power);
            }
        };
    }

    /**
     * Scale a motor's power by a constant
     *
     * @param motor the motor to scale
     * @param scale the scaling factor
     * @return the created MotorEnc
     */
    public static Motor scale(final Motor motor, final double scale) {
        return new Motor() {
            @Override
            public void setPower(double power) {
                motor.setPower(power * scale);
            }

            @Override
            public Mode getMode() {
                return motor.getMode();
            }

            @Override
            public void update() {
                motor.update();
            }

        };
    }

    /**
     * Scale a motor's power and speed by a constant
     *
     * @param motorEnc the motor to scale
     * @param scale    the scaling factor
     * @return the created MotorEnc
     */
    public static Motor scale(final MotorEnc motorEnc, final double scale) {
        return new MotorEnc() {
            @Override
            public void setSpeed(double speed) {
                motorEnc.setSpeed(speed * scale);
            }

            @Override
            public void setPosition(int encoderTarget, double maxCorrectionPower) {
                motorEnc.setPosition(encoderTarget, maxCorrectionPower);
            }

            @Override
            public void resetEncoder() {
                motorEnc.resetEncoder();
            }

            @Override
            public int getEncoderPosition() {
                return motorEnc.getEncoderPosition();
            }

            @Override
            public void setPower(double power) {
                motorEnc.setPower(power * scale);
            }

            @Override
            public Mode getMode() {
                return motorEnc.getMode();
            }

            @Override
            public void update() {
                motorEnc.update();
            }

        };
    }


    /**
     * Scale a motor's power by a function
     *
     * @param motor    the motor to scale
     * @param function the function to scale by
     * @return the created MotorEnc
     */
    public static Motor scale(final Motor motor, final Function function) {
        return new Motor() {
            @Override
            public void setPower(double power) {
                motor.setPower(function.f(power));
            }

            @Override
            public Mode getMode() {
                return motor.getMode();
            }

            @Override
            public void update() {
                motor.update();
            }

        };
    }

    /**
     * Scale a motor's power and speed by a function
     *
     * @param motorEnc the motor to scale
     * @param function the function to scale by
     * @return the created MotorEnc
     */
    public static Motor scale(final MotorEnc motorEnc, final Function function) {
        return new MotorEnc() {
            @Override
            public void setSpeed(double speed) {
                motorEnc.setSpeed(function.f(speed));
            }

            @Override
            public void setPosition(int encoderTarget, double maxCorrectionPower) {
                motorEnc.setPosition(encoderTarget, maxCorrectionPower);
            }

            @Override
            public void resetEncoder() {
                motorEnc.resetEncoder();
            }

            @Override
            public int getEncoderPosition() {
                return motorEnc.getEncoderPosition();
            }

            @Override
            public void setPower(double power) {
                motorEnc.setPower(function.f(power));
            }

            @Override
            public Mode getMode() {
                return motorEnc.getMode();
            }

            @Override
            public void update() {
                motorEnc.update();
            }

        };
    }

    /**
     * @return a motor with an encoder that does nothing
     */
    public static MotorEnc dummyWithEncoder() {
        return new MotorEnc() {
            private Mode mode = Mode.POWER;

            @Override
            public void setSpeed(double speed) {
                mode = Mode.SPEED;
            }

            @Override
            public void setPosition(int encoderTarget, double maxCorrectionPower) {
                mode = Mode.POSITION;
            }

            @Override
            public void resetEncoder() {

            }

            @Override
            public int getEncoderPosition() {
                return 0;
            }

            @Override
            public void setPower(double power) {
                mode = Mode.POWER;
            }

            @Override
            public Mode getMode() {
                return mode;
            }

            @Override
            public void update() {

            }
        };
    }

    /**
     * @return a motor that does nothing
     */
    public static Motor dummyWithoutEncoder() {
        return new Motor() {
            @Override
            public void setPower(double power) {

            }

            @Override
            public Mode getMode() {
                return Mode.POWER;
            }

            @Override
            public void update() {

            }
        };
    }

}

Before you read this, it is helpful to read about the Motor and MotorEnc interfaces and also how to create motors.

The following class groups any number of motors and can run them all at once with a list of powers.

ftc/evlib/hardware/motors/NMotors.java

package ftc.evlib.hardware.motors;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;

import ftc.electronvolts.util.Utility;
import ftc.electronvolts.util.units.Velocity;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 9/12/16
 * 
 * A general controller for a collection of N motors.
 * Knows how to run the motors given a list of Doubles.
 * Can normalize the powers/speeds before running if requested.
 * Subclasses can have fixed numbers of motors
 *
 * @see Motor
 * @see MotorEnc
 * @see OneMotors
 * @see TwoMotors
 * @see ThreeMotors
 * @see FourMotors
 * @see FiveMotors
 * @see SixMotors
 */
public class NMotors {
    /**
     * the list of motors that are grouped
     */
    private final List<? extends Motor> motors;

    /**
     * whether or not to use speed mode on the motors
     */
    private final boolean useSpeedMode;

    /**
     * the measured maximum speed of the robot
     */
    private final Velocity maxRobotSpeed;

    /**
     * the values most recently sent to the motors
     */
    private final double[] values;
    private final int[] encoders;

    /**
     * @param motors        the list of motors
     * @param useSpeedMode  true if encoder-regulated speed mode is desired
     * @param maxRobotSpeed the measured maximum speed of the robot
     */
    public NMotors(List<? extends Motor> motors, boolean useSpeedMode, Velocity maxRobotSpeed) {
        //if using speed mode, the motors need to have encoders
        if (useSpeedMode) {
            for (int i = 0; i < motors.size(); i++) {
                if (!(motors.get(i) instanceof MotorEnc)) {
                    throw new IllegalArgumentException("Argument 'motors' must be of type List<MotorEnc> if speed mode is to be used.");
                }
            }
        }
        this.motors = motors;
        this.useSpeedMode = useSpeedMode;
        this.maxRobotSpeed = maxRobotSpeed.abs();
        values = new double[motors.size()];
        encoders = new int[motors.size()];
    }

    /**
     * @return the measured maximum speed of the robot
     */
    public Velocity getMaxRobotSpeed() {
        return maxRobotSpeed;
    }

    /**
     * Allows different stop behaviors for different operations
     *
     * @param stopBehavior what to do when the power/speed is 0
     */

    /**
     * scale all the motor values if any one power is above the maximum of 1
     *
     * @param values the powers/speeds to be scaled and then run
     */
    public void runNormalized(List<Double> values) {
        if (values.size() != motors.size()) {
            throw new IllegalArgumentException("Argument 'values' must have the same length as the number of motors.");
        }

        //if the inputs are too high, scale them
        double highest = 0;

        //find the magnitude of the number with the highest magnitude
        for (double n : values) {
            if (Math.abs(n) > highest) {
                highest = Math.abs(n);
            }
        }

        if (highest < 1) { //only normalize if the values are too high
            highest = 1;
        }

        //rescale the values by the highest value
        List<Double> valuesScaled = new ArrayList<>();
        for (double power : values) {
            valuesScaled.add(power / highest);
        }

        run(valuesScaled);
    }

    /**
     * run the motors with raw power/speed
     *
     * @param values the raw values to send to the motors
     */
    public void run(List<Double> values) {
        if (values.size() != motors.size()) {
            throw new IllegalArgumentException("Argument 'values' must have the same length as the number of motors.");
        }

        //set the motor powers/speeds of each motor
        for (int i = 0; i < motors.size(); i++) {
            Double value = Utility.motorLimit(values.get(i));
            if (useSpeedMode) {
                MotorEnc motorEnc = ((MotorEnc) motors.get(i));
                motorEnc.setSpeed(value);
                encoders[i] = motorEnc.getEncoderPosition();
            } else {
                motors.get(i).setPower(value);
            }
            this.values[i] = value;
        }
    }

    /**
     * @param i the index of the motor to read the value from
     * @return the values most recently sent to the motors
     */
    public double getValue(int i) {
        return values[i];
    }

    /**
     * @param i the index of the motor to read the encoder value from
     * @return the values of the motor encoders. All encoder values will be 0 if useSpeedMode is false
     */
    public int getEncoder(int i) {
        return encoders[i];
    }

    /**
     * stop all the motors
     */
    public void stop() {
        //create a list of the same length as motors filled with zeroes
        run(new ArrayList<>(Collections.nCopies(motors.size(), 0.0)));
    }

    /**
     * Send the motor commands to the motor controller
     */
    public void update() {
        for (int i = 0; i < motors.size(); i++) {
            motors.get(i).update();
        }
    }
}

There are also similar classes for every other number of motors up to 6. (for example: FourMotors). If you need more than 6 motors, you can extend the class yourself. (You never know when you might need 800 motors for some reason.)

This class has been found to have logic errors, though the source of these has not been pinpointed. We are currently working on an alternative

Before you read this, it might be helpful to read about the NMotors class first.

Mecanum wheels use diagonal forces to move in any direction. Here is a diagram of the direction of the force of each wheel when moving forward:

To drive forward/backward, you just power all the wheels forward/backward. To turn right, you power the right wheels backward and the left wheels forward, just like normal wheels. (Turning left is the same idea.)

To move sideways right, move the front-left and back-right wheels forward, and the others backward.

Now we have this table:

            FORWARD(+x)   SIDEWAYS RIGHT(+y)   TURN RIGHT(+r)
front left      +                 +                  +
front right     +                 -                  -
back left       +                 -                  +
back right      +                 +                  -

And we can convert this table to an algorithm:

inputs: x, y, and r

flPower = + x + y + r
frPower = + x - y - r
blPower = + x - y + r
brPower = + x + y - r

This converts the desired motion of the robot into the power for the wheels.

The next thing to worry about is scaling to make sure the motors do not get sent a power that is greater than 1 or less than -1. Fortunately, the scaling is already implemented in the NMotors class.

This is the MecanumMotors class, which extends FourMotors, which in turn extends NMotors. So all MecanumMotors has to do is convert the velocity in X, Y, and R to motor powers using the algorithm we found above. It also has various setters for X, y, and R, as well as functions to convert polar (direction and distance) coordinates to cartesian (x and y) coordinates.

This example uses guava to initialize lists, which we have left for alternative methods.

ftc/evlib/hardware/motors/MecanumMotors.java

package ftc.evlib.hardware.motors;

import com.google.common.collect.ImmutableList;

import java.util.ArrayList;
import java.util.List;

import ftc.electronvolts.util.Utility;
import ftc.electronvolts.util.units.Angle;
import ftc.electronvolts.util.units.Velocity;

import static ftc.evlib.driverstation.Telem.telemetry;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 12/27/15
 *
 * A subclass of FourMotors that contains algorithms for controlling mecanum wheels.
 * It stores the X, Y, and R velocities and sends them to the motors when it is updated.
 *
 * @see FourMotors
 * @see ftc.evlib.hardware.control.MecanumControl
 */
public class MecanumMotors extends FourMotors {

    public enum MecanumDriveMode {
        NORMALIZED, TRANSLATION_NORMALIZED
    }

    //the stored velocities
    private double velocityX = 0;
    private double velocityY = 0;
    private double velocityR = 0;

    //the drive mode
    private MecanumDriveMode driveMode = MecanumDriveMode.NORMALIZED;

    /**
     * @param frontLeftMotor  the front left motor
     * @param frontRightMotor the front right motor
     * @param backLeftMotor   the back left motor
     * @param backRightMotor  the back right motor
     * @param useSpeedMode    whether or not to use speed control on the motors
     * @param maxRobotSpeed   the measured speed of the robot at 100% power
     */
    public MecanumMotors(Motor frontLeftMotor, Motor frontRightMotor, Motor backLeftMotor, Motor backRightMotor, boolean useSpeedMode, Velocity maxRobotSpeed) {
        super(frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor, useSpeedMode, maxRobotSpeed);
    }

    /**
     * @param mode the drive mode
     */
    public void setDriveMode(MecanumDriveMode mode) {
        driveMode = mode;
    }

    /**
     * Update the motor powers
     */
    public void mecanumDrive() {
        switch (driveMode) {
            case NORMALIZED:
                mecanumDriveNormalized();
                break;
            case TRANSLATION_NORMALIZED:
                mecanumDriveTranslationNormalized();
                break;
        }
    }

    /**
     * run the motors based on the xyr velocities
     * normalize if any motor power is too large
     */
    private void mecanumDriveNormalized() {
        //calculate motor powers
        runMotorsNormalized(
                velocityX + velocityY - velocityR,
                velocityX - velocityY + velocityR,
                velocityX - velocityY - velocityR,
                velocityX + velocityY + velocityR
        );
    }

    /**
     * Calculate rotational velocity first, and use remaining headway for translation.
     */
    private void mecanumDriveTranslationNormalized() {
        //calculate motor powers
        List<Double> translationValues = ImmutableList.of(
                velocityX + velocityY,
                velocityX - velocityY,
                velocityX - velocityY,
                velocityX + velocityY);

        List<Double> rotationValues = ImmutableList.of(
                -velocityR,
                velocityR,
                -velocityR,
                velocityR);

        double scaleFactor = 1;
        double tmpScale = 1;

        // Solve this equation backwards:
        // MotorX = TranslationX * scaleFactor + RotationX
        // to find scaleFactor that ensures -1 <= MotorX <= 1 and 0 < scaleFactor <= 1

        for (int i = 0; i < 4; i++) {
            if (Math.abs(translationValues.get(i) + rotationValues.get(i)) > 1) {
                tmpScale = (1 - rotationValues.get(i)) / translationValues.get(i);
            } else if (translationValues.get(i) + rotationValues.get(i) < -1) {
                tmpScale = (rotationValues.get(i) - 1) / translationValues.get(i);
            }
            if (tmpScale < scaleFactor) {
                scaleFactor = tmpScale;
            }
        }

        telemetry.addData("driveMode", driveMode.toString());
        telemetry.addData("scaleFactor", scaleFactor);

        List<Double> valuesScaled = new ArrayList<>();
        for (int i = 0; i < 4; i++) {
            valuesScaled.add(translationValues.get(i) * scaleFactor + rotationValues.get(i));
            telemetry.addData("valuesScaled(" + i + ")", valuesScaled.get(i));
        }

        run(valuesScaled);
    }

    /**
     * @param velocityX the x velocity
     */
    public void setVelocityX(double velocityX) {
        this.velocityX = Utility.motorLimit(velocityX);
    }

    /**
     * @param velocityY the y velocity
     */
    public void setVelocityY(double velocityY) {
        this.velocityY = Utility.motorLimit(velocityY);
    }

    /**
     * @param velocityR the rotational velocity
     */
    public void setVelocityR(double velocityR) {
        this.velocityR = Utility.motorLimit(velocityR);
    }

    /**
     * set the x and y velocities at the same time
     *
     * @param velocityX the x velocity
     * @param velocityY the y velocity
     */
    public void setVelocityXY(double velocityX, double velocityY) {
        setVelocityX(velocityX);
        setVelocityY(velocityY);
    }

    /**
     * set the x, y, and rotational velocities at the same time
     *
     * @param velocityX the x velocity
     * @param velocityY the y velocity
     * @param velocityR the rotational velocity
     */
    public void setVelocityXYR(double velocityX, double velocityY, double velocityR) {
        setVelocityX(velocityX);
        setVelocityY(velocityY);
        setVelocityR(velocityR);
    }

    /**
     * set the x and y velocities using polar coordinates
     *
     * @param velocity  the velocity (r of the polar coordinate)
     * @param direction the direction (theta of the polar coordinate)
     */
    public void setVelocityPolar(double velocity, Angle direction) {
        double directionRadians = direction.radians();
        setVelocityX(velocity * Math.cos(directionRadians));
        setVelocityY(velocity * Math.sin(directionRadians));
    }

    /**
     * set the x and y velocities using polar coordinates, and also set the rotational velocity
     *
     * @param velocity  the velocity (r of the polar coordinate)
     * @param direction the direction (theta of the polar coordinate)
     * @param velocityR the rotational velocity
     */
    public void setVelocityPolarR(double velocity, Angle direction, double velocityR) {
        setVelocityPolar(velocity, direction);
        setVelocityR(velocityR);
    }
}

This class has been found to have logic errors, though the source of these has not been pinpointed. We are currently working on an alternative

Using the MecanumMotors class that converts velocity in the X, Y, and R directions, we need a way to add control algorithms, such as gyro stabilization or line following.

The gyro algorithm would look like:

Loop:
  get current heading from gyro
  calculate heading error from desired heading
  adjust ROTATION based on heading error

And the line following would look like: (The x axis is the forward/backward movement of the robot)

Loop:
  get values from line sensors
  calculate position on the line (LEFT, MIDDLE, or RIGHT)
  if LEFT: y = positive
  if middle: y = 0
  if RIGHT: y = negative
  x is defined by the input
  set the TRANSLATION to (x, y)

As you can see, the gyro stabilization controls the rotation, and the line following controls the translation.

Now the challenge is to make a class that can accept different types of translation/rotation controllers.

First, we create interfaces TranslationControl and RotationControl to define what they have output:

ftc/evlib/hardware/control/TranslationControl.java

package ftc.evlib.hardware.control;

import ftc.electronvolts.util.Vector2D;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 9/19/16
 * 
 * Controls the translation of a mecanum robot
 *
 * @see TranslationControls
 * @see Vector2D
 */
public interface TranslationControl {
    /**
     * update the velocity x and velocity y values
     *
     * @return true if there were no problems
     */
    boolean act();

    /**
     * @return the translational velocity
     */
    Vector2D getTranslation();
}

As you can see, the translation control returns a Vector2D (x and y).

RotationControl is more complicated, because it not only has to find the angular velocity, but also apply a correction of a certain angle to the translation.

ftc/evlib/hardware/control/RotationControl.java

package ftc.evlib.hardware.control;

import ftc.electronvolts.util.units.Angle;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 9/19/16
 * 
 * Controls the rotation of a mecanum robot
 *
 * @see RotationControls
 * @see Angle
 */
public interface RotationControl {
    double DEFAULT_MAX_ANGULAR_SPEED = 0.5;
//    double DEFAULT_MAX_ANGULAR_SPEED = 0.8;

    /**
     * update the rotational velocity
     *
     * @return true if there were no problems
     */
    boolean act();

    /**
     * @return the rotational velocity
     */
    double getVelocityR();

    /**
     * accounts for the robot's rotation being off when translating
     *
     * @return correction to the translation direction
     */
    Angle getPolarDirectionCorrection();
}

There are factory classes for the TranslationControl and RotationControl: ftc/evlib/hardware/control/TranslationControls.java ftc/evlib/hardware/control/RotationControls.java

They create the controllers such as the gyro stabilization, line following, and also controllers that return constant values for dead reckoning.

For controllers that control the rotation and translation, there is an abstract class called XYRControl that combines both the RotationControl and TranslationControl interfaces.

ftc/evlib/hardware/control/XYRControl.java

package ftc.evlib.hardware.control;

import ftc.electronvolts.util.units.Angle;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 12/14/16
 *
 * Controls both the translation (x and y) and the rotation (r) of a robot with mecanum wheels
 *
 * @see RotationControl
 * @see TranslationControl
 * @see XYRControls
 */
public abstract class XYRControl implements RotationControl, TranslationControl {
    @Override
    public Angle getPolarDirectionCorrection() {
        return Angle.zero();
    }
}

The factory class for XYRControl is ftc/evlib/hardware/control/XYRControls.java

Now we need to take the value from the TranslationControl and RotationControl and pass them to the MecanumMotors. This is done by MecanumControl:

ftc/evlib/hardware/control/MecanumControl.java

package ftc.evlib.hardware.control;

import ftc.electronvolts.util.Vector2D;
import ftc.electronvolts.util.units.Velocity;
import ftc.evlib.hardware.motors.MecanumMotors;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 9/19/16
 * 
 * Manages what algorithms control the rotation and translation of the mecanum wheels
 * This allows you to mix and match rotation and translation (x and y) controllers and change them whenever you want
 *
 * @see MecanumMotors
 * @see RotationControl
 * @see RotationControls
 * @see TranslationControl
 * @see TranslationControls
 */
public class MecanumControl {
    /**
     * the motors to be controlled
     */
    private final MecanumMotors mecanumMotors;

    //the controllers for the rotation and translation
    private RotationControl rotationControl;
    private TranslationControl translationControl;

    /**
     * the velocity in the x, y, and rotation directions
     */
    private double velocityX, velocityY, velocityR;

    /**
     * stores whether or not the translation/rotation worked
     * for example, translationWorked will be false if the line following lost the line
     */
    private boolean translationWorked = true, rotationWorked = false;

    /**
     * create a MecanumControl that is not moving
     *
     * @param mecanumMotors the motors to control
     */
    public MecanumControl(MecanumMotors mecanumMotors) {
        this(mecanumMotors, XYRControls.ZERO);
    }

    /**
     * create a MecanumControl and immediately start using an XYR control
     *
     * @param mecanumMotors the motors to control
     * @param xyrControl    how to command the motors' rotation and translation
     */
    public MecanumControl(MecanumMotors mecanumMotors, XYRControl xyrControl) {
        this(mecanumMotors, xyrControl, xyrControl);
    }

    /**
     * create a MecanumControl and immediately start using TranslationControl and RotationControl
     *
     * @param mecanumMotors      the motors to control
     * @param rotationControl    how to command the motors' rotation
     * @param translationControl how to command the motors' translation
     */
    public MecanumControl(MecanumMotors mecanumMotors, RotationControl rotationControl, TranslationControl translationControl) {
        this.rotationControl = rotationControl;
        this.mecanumMotors = mecanumMotors;
        this.translationControl = translationControl;
    }

    /**
     * @return the robot's speed when the power is set to 100%
     */
    public Velocity getMaxRobotSpeed() {
        return mecanumMotors.getMaxRobotSpeed();
    }

    /**
     * @param rotationControl the controller that determines the robot's rotation
     */
    public void setRotationControl(RotationControl rotationControl) {
        this.rotationControl = rotationControl;
    }

    /**
     * @param translationControl the controller that determines the robot's translation (x and y)
     */
    public void setTranslationControl(TranslationControl translationControl) {
        this.translationControl = translationControl;
    }

    /**
     * set both translation and rotation controls at once
     *
     * @param translationControl the controller that determines the robot's translation (x and y)
     * @param rotationControl    the controller that determines the robot's rotation
     */
    public void setControl(TranslationControl translationControl, RotationControl rotationControl) {
        this.translationControl = translationControl;
        this.rotationControl = rotationControl;
    }

    /**
     * set both translation and rotation controls at once
     *
     * @param rotationControl    the controller that determines the robot's rotation
     * @param translationControl the controller that determines the robot's translation (x and y)
     */
    public void setControl(RotationControl rotationControl, TranslationControl translationControl) {
        this.translationControl = translationControl;
        this.rotationControl = rotationControl;
    }

    /**
     * Set both translation and rotation controls to the same object
     *
     * @param xyrControl the controller that determines both the translation and rotation
     */
    public void setControl(XYRControl xyrControl) {
        this.translationControl = xyrControl;
        this.rotationControl = xyrControl;
    }

    /**
     * stop the motors
     */
    public void stop() {
        translationControl = TranslationControls.ZERO;
        rotationControl = RotationControls.ZERO;
        mecanumMotors.stop();
    }

    /**
     * set the drive mode to turn on or off translation normalizing
     *
     * @param mode the drive mode
     */
    public void setDriveMode(MecanumMotors.MecanumDriveMode mode) {
        mecanumMotors.setDriveMode(mode);
    }

    /**
     * update the motor powers based on the output of the translationControl and rotationControl
     */
    public void act() {
        translationWorked = translationControl.act();

        //in case the same object is passed in that implements both controllers
        if (translationControl == rotationControl) {
            rotationWorked = translationWorked;
        } else {
            rotationWorked = rotationControl.act();
        }

        Vector2D translation = translationControl.getTranslation();

        double velocity = translation.getLength();
        double directionRads = translation.getDirection().radians() +
                rotationControl.getPolarDirectionCorrection().radians();

        velocityX = velocity * Math.cos(directionRads);
        velocityY = velocity * Math.sin(directionRads);

        velocityR = rotationControl.getVelocityR();

        mecanumMotors.setVelocityXYR(
                velocityX,
                velocityY,
                velocityR
        );

        mecanumMotors.mecanumDrive();
        mecanumMotors.update();
    }

    /**
     * @return whether or not the translation worked
     */
    public boolean translationWorked() {
        return translationWorked;
    }

    /**
     * @return whether or not the rotation worked
     */
    public boolean rotationWorked() {
        return rotationWorked;
    }

    /**
     * @return the robot's x velocity
     */
    public double getVelocityX() {
        return velocityX;
    }

    /**
     * @return the robot's y velocity
     */
    public double getVelocityY() {
        return velocityY;
    }

    /**
     * @return the robot's rotational velocity
     */
    public double getVelocityR() {
        return velocityR;
    }

    public RotationControl getRotationControl() {
        return rotationControl;
    }

    public TranslationControl getTranslationControl() {
        return translationControl;
    }
}

EVLib has its own interface for Analog sensors. It extends InputExtractor (from the state-machine-framework) of type Double, which means that every AnalogSensor has to have a method that returns a Double: Double getValue().

ftc/evlib/hardware/sensors/AnalogSensor.java

package ftc.evlib.hardware.sensors;

import ftc.electronvolts.util.InputExtractor;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 9/11/16
 * 
 * Interface for any type of analog sensor
 * examples: light sensor, distance sensor, potentiometer
 *
 * @see InputExtractor
 * @see Sensors
 */
public interface AnalogSensor extends InputExtractor<Double> {
}

To create an AnalogSensor from an AnalogInput (which is part of the FTC libraries), you can use the ftc/evlib/hardware/sensors/Sensors.java factory class:

AnalogInput analogInput = hardwareMap.analogInput.get("lightSensor");
AnalogSensor lightSensor = Sensors.analog(analogInput);

//To combine on one line:
AnalogSensor lightSensor = Sensors.analog(hardwareMap, "lightSensor");

To get the value of a sensor, call the getValue() method:

telemetry.addData("light sensor", lightSensor.getValue());

See also: DistanceSensor, Digital Sensors

AveragedSensor takes an AnalogSensor and averages the last N values to get the current value. The act() method should be called exactly once every time through the loop, but getValue() can be called whenever you need the value. The isReady() method will return true if the sensor is done collecting the first N values. If the sensor is not ready, the value will not be accurate.

ftc/evlib/hardware/sensors/AveragedSensor.java

package ftc.evlib.hardware.sensors;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 3/16/16
 * 
 * report the average of the last N values of a sensor as the value
 *
 * @see AnalogSensor
 */
public class AveragedSensor implements AnalogSensor {
    private final AnalogSensor sensor;
    private final double readings[];
    private int index = 0;
    private boolean ready = false;
    private final int numReadings;
    private double average = 0;

    /**
     * @param sensor      the sensor to average
     * @param numReadings the number of readings to average
     */
    public AveragedSensor(AnalogSensor sensor, int numReadings) {
        this.sensor = sensor;
        this.numReadings = numReadings;
        readings = new double[numReadings];
    }

    /**
     * @return true once the first numReadings have been read
     */
    public boolean isReady() {
        return ready;
    }

    @Override
    public Double getValue() {
        return average;
    }

    /**
     * read the sensor and update the average
     */
    public void act() {
        double reading = sensor.getValue();
        readings[index] = reading;
        index++;
        if (index >= numReadings) {
            index = 0;
            ready = true;
        }
        if (ready) {
            average = 0;
            for (int i = 0; i < numReadings; i++) {
                average += readings[i];
            }
            average /= numReadings;
        } else {
            average = reading;
        }
    }
}

EVLib has its own interface for Digital sensors. It extends InputExtractor of type Boolean, which means that every DigitalSensor has to have a getValue method that returns a Boolean.

ftc/evlib/hardware/sensors/DigitalSensor.java

package ftc.evlib.hardware.sensors;

import ftc.electronvolts.util.InputExtractor;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 9/12/16
 * 
 * Interface for any type of digital sensor
 * examples: touch sensor, digital line sensor, magnetic reed switch
 *
 * @see InputExtractor
 * @see Sensors
 */
public interface DigitalSensor extends InputExtractor<Boolean> {
}

You can create a DigitalSensor from a DigitalChannel or TouchSensor using the Sensors factory class:

DigitalInput digitalInput = hardwareMap.touchSensor.get("limitSwitch");
DigitalSensor limitSwitch = Sensors.digitalSensor(digitalInput);

//To combine on one line:
DigitalSensor button = Sensors.digitalSensor(hardwareMap, "button");

To get the value of a DigitalSensor, call the getValue() method which returns a Boolean.

teletry.addData("limit switch pressed?", limitSwitch.getValue());

See also: Analog Sensors

CalibratedLineSensor takes an analog line sensor and turns it into a DigitalSensor by finding a threshold between light and dark. The getValue() method will return true if the sensor sees a line. Note: the act method must be called every loop to update the sensor values for averaging. This class is used in DoubleLineSensor.

ftc/evlib/hardware/sensors/CalibratedLineSensor.java

package ftc.evlib.hardware.sensors;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 3/8/16
 * 
 * manages threshold calculation for the line sensor
 *
 * @see AnalogSensor
 */
public class CalibratedLineSensor implements DigitalSensor {
    private boolean ready = true;
    private final AnalogSensor lineSensor;
    private static final int NUM_CALIBRATIONS = 100;
    private int numReadings;
    private double average, deviation, threshold = 80;
    private final double readings[] = new double[NUM_CALIBRATIONS];
    private boolean seeingLine = false;
    private double value;

    /**
     * @param lineSensor the raw line sensor
     */
    public CalibratedLineSensor(AnalogSensor lineSensor) {
        this.lineSensor = lineSensor;
    }

    /**
     * start calibrating the sensor
     */
    public void calibrate() {
        ready = false;
        average = 0;
        numReadings = 0;
        deviation = 0;
    }

    /**
     * @return true when done calibrating
     */
    public boolean isReady() {
        return ready;
    }

    /**
     * @return true if the sensor is over the line
     */
    @Override
    public Boolean getValue() {
        return seeingLine;
    }

    /**
     * read the sensor value and use it to calibrate/update the average
     */
    public void act() {
        value = lineSensor.getValue();
        if (!ready) {
            if (numReadings < NUM_CALIBRATIONS) {
                //add a value
                readings[numReadings] = value;
                numReadings++;

                //update the average
                average = (average * (numReadings - 1) + value) / (numReadings);

            } else {
                //find deviation
                for (double reading : readings) {
                    deviation += (reading - average) * (reading - average);
                }
                deviation /= Math.sqrt(numReadings);

                threshold = (average - deviation) * (average - deviation) / average;
                ready = true;
            }
        }
        seeingLine = value < threshold;
    }

    /**
     * @return the raw value of the line sensor
     */
    public double getRawValue() {
        return value;
    }
}

This takes two CalibratedLineSensor objects and combines them. It reports the robot's position relative to the line. It is used in LineFinder along with a ColorSensor to find different colored lines.

It is be used in ftc/evlib/hardware/control/TranslationControls.java to do line following. See: Mecanum Control

ftc/evlib/hardware/sensors/DoubleLineSensor.java

package ftc.evlib.hardware.sensors;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 3/8/16
 * 
 * Manages two line sensors to tell where the robot is relative to the line
 *
 * @see CalibratedLineSensor
 * @see DigitalSensor
 */
public class DoubleLineSensor implements DigitalSensor {

    public enum LinePosition {
        LEFT,
        MIDDLE,
        RIGHT,
        OFF_UNKNOWN,
        OFF_LEFT,
        OFF_RIGHT
    }

    private LinePosition position = LinePosition.OFF_UNKNOWN;

    private final CalibratedLineSensor leftSensor, rightSensor;

    public DoubleLineSensor(AnalogSensor leftSensor, AnalogSensor rightSensor) {
        this.leftSensor = new CalibratedLineSensor(leftSensor);
        this.rightSensor = new CalibratedLineSensor(rightSensor);
    }

    public void calibrate() {
        leftSensor.calibrate();
        rightSensor.calibrate();
    }

    public boolean isReady() {
        return leftSensor.isReady() && rightSensor.isReady();
    }

    public LinePosition getPosition() {
        return position;
    }

    public void reset() {
        position = LinePosition.MIDDLE;
    }

    /**
     * @return true if either sensor is on the line
     */
    @Override
    public Boolean getValue() {
        return leftSensor.getValue() || rightSensor.getValue();
    }

    /**
     * determine where the robot is relative to the line
     */
    public void act() {
        leftSensor.act();
        rightSensor.act();

        if (leftSensor.getValue()) {
            if (rightSensor.getValue()) {
                position = LinePosition.MIDDLE;
            } else {
                position = LinePosition.LEFT;
            }
        } else {
            if (rightSensor.getValue()) {
                position = LinePosition.RIGHT;
            } else {
                //if neither sensor detects the line, the position is determined by the previous position
                if (position == LinePosition.LEFT) {
                    position = LinePosition.OFF_LEFT;
                } else if (position == LinePosition.RIGHT) {
                    position = LinePosition.OFF_RIGHT;
                } else {
                    position = LinePosition.OFF_UNKNOWN;
                }
            }
        }
    }
}

ColorSensor uses a red LED, a blue LED, and a photoresistor to detect the color of a line on the mat. It has 4 methods for setting the color: red, blue, magenta, and off. It implements DigitalSensor, returning whether or not it is seeing the color specified. It is used in LineFinder along with DoubleLineSensor to more accurately find colored lines.

Fun fact: last year when connecting LEDs, we guessed on the resistor values and it burnt out 2 of our digital outputs. Always calculate the current before you plug LEDs in to the Core Device Interface Module.

ftc/evlib/hardware/sensors/ColorSensor.java

package ftc.evlib.hardware.sensors;

import com.qualcomm.robotcore.hardware.LED;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 3/22/16
 * 
 * Uses LEDs and a photoresistor to detect the color of a line on the mat
 * 
 * --- light red blue
 * mat    35 250 250
 * red
 * blue
 * white 145 640 550
 *
 * @see DigitalSensor
 */
public class ColorSensor implements DigitalSensor {
    private static final int THRESHOLD = 500;
    private final LED redLight, blueLight;
    private final AveragedSensor lightSensor;
    private String ledColorString = "unknown";

    /**
     * @param redLight    the red LED
     * @param blueLight   the blue LED
     * @param lightSensor the photoresistor
     */
    public ColorSensor(LED redLight, LED blueLight, AveragedSensor lightSensor) {
        this.redLight = redLight;
        this.blueLight = blueLight;
        this.lightSensor = lightSensor;
    }

    /**
     * turn off both LEDs
     */
    public void turnOff() {
        redLight.enable(true);
        blueLight.enable(true);
        ledColorString = "blank";
    }

    /**
     * turn on the red LED and turn off the blue LED
     */
    public void setColorRed() {
        redLight.enable(false);
        blueLight.enable(true);
        ledColorString = "red";
    }

    /**
     * turn on the blue LED and turn off the red LED
     */
    public void setColorBlue() {
        redLight.enable(true);
        blueLight.enable(false);
        ledColorString = "blue";
    }

    /**
     * turn on both LEDs
     */
    public void setColorMagenta() {
        redLight.enable(false);
        blueLight.enable(false);
        ledColorString = "magenta";
    }

    /**
     * @return the name of the current color for telemetry purposes
     */
    public String getLedColorString() {
        return ledColorString;
    }

    /**
     * update the light sensor
     */
    public void act() {
        lightSensor.act();
    }

    /**
     * @return true if it is seeing the color requested
     */
    @Override
    public Boolean getValue() {
        return lightSensor.getValue() > THRESHOLD;
    }

    /**
     * @return the raw value of the light sensor
     */
    public double getRawValue() {
        return lightSensor.getValue();
    }
}

LineFinder uses a DoubleLineSensor and a ColorSensor to find lines of different colors. You use it by setting the color you are looking for, then calling getValue() to see if it is seeing the line color you requested.

ftc/evlib/hardware/sensors/LineFinder.java

package ftc.evlib.hardware.sensors;


import static ftc.evlib.driverstation.Telem.telemetry;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 4/10/16
 * 
 * Combines two line sensors and an LED-powered color sensor to detect the color of lines
 *
 * @see ColorSensor
 * @see DoubleLineSensor
 */
public class LineFinder implements DigitalSensor {
    public enum LineColor {
        RED,
        BLUE,
        WHITE_RED_TEAM,
        WHITE_BLUE_TEAM
    }

    private final ColorSensor colorSensor;
    private final DoubleLineSensor doubleLineSensor;
    private LineColor lookingFor = LineColor.WHITE_RED_TEAM;
    private boolean justStarted = true;


    public LineFinder(ColorSensor colorSensor, DoubleLineSensor doubleLineSensor) {
        this.colorSensor = colorSensor;
        this.doubleLineSensor = doubleLineSensor;
    }

    /**
     * start looking for a line of a certain color by turning the LEDs to a certain color
     *
     * @param lineColor the color of the line to look for
     */
    public void startLookingFor(LineColor lineColor) {
        lookingFor = lineColor;
        justStarted = true;
        if (lookingFor == LineColor.RED) {
            colorSensor.setColorBlue();
        } else if (lookingFor == LineColor.BLUE) {
            colorSensor.setColorRed();
        } else if (lookingFor == LineColor.WHITE_RED_TEAM) {
            colorSensor.setColorBlue();
        } else if (lookingFor == LineColor.WHITE_BLUE_TEAM) {
            colorSensor.setColorRed();
        }
    }

    /**
     * note: it skips the first loop because the LEDs take 1 loop to turn on
     *
     * @return true if the line of the requested color was found
     */
    @Override
    public Boolean getValue() {
        if (justStarted) {
            justStarted = false;
            telemetry.addData("Just started line finder", "");
        } else {
            if (lookingFor == LineColor.RED) {
                return doubleLineSensor.getValue() && !colorSensor.getValue();
            } else if (lookingFor == LineColor.BLUE) {
                return doubleLineSensor.getValue() && !colorSensor.getValue();
            } else if (lookingFor == LineColor.WHITE_RED_TEAM) {
                return doubleLineSensor.getValue() && colorSensor.getValue();
            } else if (lookingFor == LineColor.WHITE_BLUE_TEAM) {
                return doubleLineSensor.getValue() && colorSensor.getValue();
            }
        }
        return false;
    }

    public void act() {
        doubleLineSensor.act();
        colorSensor.act();
    }
}

The GamepadManager adds edge detection to all the gamepad buttons and analog scaling to the joysticks and left/right triggers.

You can use it by first creating a new GamepadManager. You have to pass in the gamepad and a Function to scale the analog values with.

driver1 = new GamepadManager(gamepad1, Functions.squared());

Then to use it, for example to tell if button "a" has just been pressed:

if (driver1.a.justPressed()){
    //do something
}

java/ftc/evlib/driverstation/GamepadManager.java

package ftc.evlib.driverstation;

import com.qualcomm.robotcore.hardware.Gamepad;

import ftc.electronvolts.util.AnalogInputScaler;
import ftc.electronvolts.util.DigitalInputEdgeDetector;
import ftc.electronvolts.util.Function;
import ftc.electronvolts.util.Functions;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 1/9/16
 * 
 * This class wraps a gamepad and adds:
 * Edge detection to the digital inputs (buttons and dpad) {@see DigitalInputEdgeDetector}
 * Scaling to the analog inputs (joysticks and triggers) {@see AnalogInputScaler}
 */
public class GamepadManager {
    //this stores all the wrapped digital inputs
    public final DigitalInputEdgeDetector a, b, x, y, left_bumper, right_bumper,
            dpad_up, dpad_down, dpad_left, dpad_right,
            left_stick_button, right_stick_button, back, start;

    //this stores all the wrapped analog inputs
    public final AnalogInputScaler left_stick_x, left_stick_y, right_stick_x, right_stick_y,
            left_trigger, right_trigger;


    //use this constructor for no joystick scaling
    public GamepadManager(Gamepad gamepad) {
        this(gamepad, Functions.none());
    }

    //use this constructor for custom joystick scaling
    public GamepadManager(Gamepad gamepad, Function scalingFunction) {
        //create all the DigitalInputEdgeDetector objects
        a = new DigitalInputEdgeDetector(GamepadIEFactory.a(gamepad));
        b = new DigitalInputEdgeDetector(GamepadIEFactory.b(gamepad));
        x = new DigitalInputEdgeDetector(GamepadIEFactory.x(gamepad));
        y = new DigitalInputEdgeDetector(GamepadIEFactory.y(gamepad));
        left_bumper = new DigitalInputEdgeDetector(GamepadIEFactory.left_bumper(gamepad));
        right_bumper = new DigitalInputEdgeDetector(GamepadIEFactory.right_bumper(gamepad));
        dpad_up = new DigitalInputEdgeDetector(GamepadIEFactory.dpad_up(gamepad));
        dpad_down = new DigitalInputEdgeDetector(GamepadIEFactory.dpad_down(gamepad));
        dpad_left = new DigitalInputEdgeDetector(GamepadIEFactory.dpad_left(gamepad));
        dpad_right = new DigitalInputEdgeDetector(GamepadIEFactory.dpad_right(gamepad));
        left_stick_button = new DigitalInputEdgeDetector(GamepadIEFactory.left_stick_button(gamepad));
        right_stick_button = new DigitalInputEdgeDetector(GamepadIEFactory.right_stick_button(gamepad));
        back = new DigitalInputEdgeDetector(GamepadIEFactory.back(gamepad));
        start = new DigitalInputEdgeDetector(GamepadIEFactory.start(gamepad));

        //create all the AnalogInputScaler objects
        left_stick_x = new AnalogInputScaler(GamepadIEFactory.left_stick_x(gamepad), scalingFunction);
        left_stick_y = new AnalogInputScaler(GamepadIEFactory.left_stick_y(gamepad), scalingFunction);
        right_stick_x = new AnalogInputScaler(GamepadIEFactory.right_stick_x(gamepad), scalingFunction);
        right_stick_y = new AnalogInputScaler(GamepadIEFactory.right_stick_y(gamepad), scalingFunction);
        left_trigger = new AnalogInputScaler(GamepadIEFactory.left_trigger(gamepad), scalingFunction);
        right_trigger = new AnalogInputScaler(GamepadIEFactory.right_trigger(gamepad), scalingFunction);
    }

    public void update() {
        //update all the values
        a.update();
        b.update();
        x.update();
        y.update();

        left_bumper.update();
        right_bumper.update();

        left_trigger.update();
        right_trigger.update();

        dpad_up.update();
        dpad_down.update();
        dpad_left.update();
        dpad_right.update();

        left_stick_button.update();
        right_stick_button.update();

        back.update();
        start.update();

        left_stick_x.update();
        left_stick_y.update();

        right_stick_x.update();
        right_stick_y.update();


        /*telemetry.addData("a", a.isPressed());
        telemetry.addData("b", b.isPressed());
        telemetry.addData("x", x.isPressed());
        telemetry.addData("y", y.isPressed());
        telemetry.addData("LB", left_bumper.isPressed());
        telemetry.addData("RB", right_bumper.isPressed());
        telemetry.addData("DU", dpad_up.isPressed());
        telemetry.addData("DD", dpad_down.isPressed());
        telemetry.addData("DL", dpad_left.isPressed());
        telemetry.addData("DR", dpad_right.isPressed());
        telemetry.addData("LSB", left_stick_button.isPressed());
        telemetry.addData("RSB", right_stick_button.isPressed());
        telemetry.addData("Back", back.isPressed());
        telemetry.addData("Start", start.isPressed());

        telemetry.addData("Left X", left_stick_x.isPressed());
        telemetry.addData("Left Y", left_stick_y.isPressed());
        telemetry.addData("Right X", right_stick_x.isPressed());
        telemetry.addData("Right Y", right_stick_y.isPressed());
        telemetry.addData("Left Trigger", left_trigger.isPressed());
        telemetry.addData("Right Trigger", right_trigger.isPressed());
        */
    }
}

Angle | Distance | Time | Velocity | AngularVelocity

Units: radians, degrees, rotations

The Angle class stores an angle. It can be created from any of the units above, and you can get the value from it in any of the units above. Once the Angle is created, the value cannot be changed.

Angle has a few math functions: abs (absolute value), signum (returns the sign), add, subtract, multiply, and divide.

ftc/electronvolts/util/units/Angle.java

package ftc.electronvolts.util.units;


/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 2/2/16
 */
public class Angle {
    /*
     * Constants to relate various units to radians
     * The reciprocal of each constant is calculated once to be used multiple
     * times later
     */
    private static double RAD_PER_DEG = Math.PI / 180;
    private static double RAD_PER_ROT = 2 * Math.PI;

    private static double DEG_PER_RAD = 1 / RAD_PER_DEG;
    private static double ROT_PER_RAD = 1 / RAD_PER_ROT;
    
    private static Angle zero = new Angle(0);

    // holds the angle's value in radians
    private final double radians;

    /**
     * private constructor to make an Angle object from a value in radians
     *
     * @param radians the angle's value in radians
     */
    private Angle(double radians) {
        this.radians = radians;
    }

    /**
     * 
     * @return the absolute value of the angle
     */
    public Angle abs() {
        return new Angle(Math.abs(radians));
    }

    /**
     * 
     * @return the sign of the angle
     */
    public double signum() {
        return Math.signum(radians);
    }

    /**
     * Adds two Angles together
     *
     * @param angle1 one Angle
     * @param angle2 another Angle
     * @return the resulting Angle
     */
    public static Angle add(Angle angle1, Angle angle2) {
        return new Angle(angle1.radians + angle2.radians);
    }

    /**
     * Subtracts angle2 from angle1
     * 
     * @param angle1 one Angle
     * @param angle2 another Angle
     * @return the resulting Angle
     */
    public static Angle subtract(Angle angle1, Angle angle2) {
        return new Angle(angle1.radians - angle2.radians);
    }

    /**
     * Multiplies an Angle by a number
     * 
     * @param angle the Angle to be multiplied
     * @param number the number to multiply by
     * @return the resulting Angle
     */
    public static Angle multiply(Angle angle, double number) {
        return new Angle(angle.radians * number);
    }

    /**
     * Divides an Angle by a number
     * 
     * @param angle the Angle to be multiplied
     * @param number the number to divided by
     * @return the resulting Angle
     */
    public static Angle divide(Angle angle, double number) {
        return new Angle(angle.radians / number);
    }

    /**
     * Creates an Angle that has a value of 0 (in all units)
     * 
     * @return the created Angle
     */
    public static Angle zero() {
        return zero;
    }

    @Override
    public int hashCode() {
        final int prime = 31;
        int result = 1;
        long temp;
        temp = Double.doubleToLongBits(radians);
        result = prime * result + (int) (temp ^ (temp >>> 32));
        return result;
    }

    @Override
    public boolean equals(Object obj) {
        if (this == obj) return true;
        if (obj == null) return false;
        if (getClass() != obj.getClass()) return false;
        Angle other = (Angle) obj;
        if (Double.doubleToLongBits(radians) != Double.doubleToLongBits(other.radians)) return false;
        return true;
    }

    /**
     * Creates an Angle class from a value in radians
     *
     * @param v the value in radians
     * @return the Angle class
     */
    public static Angle fromRadians(double v) {
        return new Angle(v);
    }

    /**
     * Creates an Angle class from a value in degrees
     *
     * @param v the value in degrees
     * @return the Angle class
     */
    public static Angle fromDegrees(double v) {
        return new Angle(v * RAD_PER_DEG);
    }

    /**
     * Creates an Angle class from a value in rotations
     *
     * @param v the value in rotations
     * @return the Angle class
     */
    public static Angle fromRotations(double v) {
        return new Angle(v * RAD_PER_ROT);
    }

    /**
     * 
     * @return the value of the Angle in radians
     */
    public double radians() {
        return radians;
    }

    /**
     * 
     * @return the value of the Angle in degrees
     */
    public double degrees() {
        return radians * DEG_PER_RAD;
    }

    /**
     * 
     * @return the value of the Angle in rotations
     */
    public double rotations() {
        return radians * ROT_PER_RAD;
    }
}

Angle | Distance | Time | Velocity | AngularVelocity

Units: meters, (kilo|centi|milli|micro|nano)meters, feet, inches, yards, miles, nautical miles

The Distance class stores an amount of distance. It can be created from any of the units above, and you can get the value from it in any of the units above. Once the Distance is created, the value cannot be changed.

Distance has a few math functions: abs (absolute value), signum (returns the sign), add, subtract, multiply, and divide.

ftc/electronvolts/util/units/Distance.java

package ftc.electronvolts.util.units;


/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/4/16
 */

public class Distance {
    /*
     * Constants to relate various units to meters
     * The reciprocal of each constant is calculated once to be used multiple
     * times later
     */
    private static final double M_PER_KM = 1e+3;
    private static final double M_PER_CM = 1e-2;
    private static final double M_PER_MM = 1e-3;
    private static final double M_PER_UM = 1e-6;
    private static final double M_PER_NM = 1e-9;
    private static final double M_PER_FT = 0.3048;
    private static final double M_PER_IN = 0.0254;
    private static final double M_PER_YD = 0.9144;
    private static final double M_PER_MI = 1609.344;
    private static final double M_PER_NAUT_MI = 1852;

    private static final double KM_PER_M = 1 / M_PER_KM;
    private static final double CM_PER_M = 1 / M_PER_CM;
    private static final double MM_PER_M = 1 / M_PER_MM;
    private static final double UM_PER_M = 1 / M_PER_UM;
    private static final double NM_PER_M = 1 / M_PER_NM;
    private static final double FT_PER_M = 1 / M_PER_FT;
    private static final double IN_PER_M = 1 / M_PER_IN;
    private static final double YD_PER_M = 1 / M_PER_YD;
    private static final double MI_PER_M = 1 / M_PER_MI;
    private static final double NAUT_MI_PER_M = 1 / M_PER_NAUT_MI;
    
    private static final Distance zero = new Distance(0);

    // The distance in meters
    private final double meters;

    /**
     * Private constructor to create a Distance object from meters
     * 
     * @param meters the number of meters
     */
    private Distance(double meters) {
        this.meters = meters;
    }

    /**
     * 
     * @return the absolute value of the distance
     */
    public Distance abs() {
        return new Distance(Math.abs(meters));
    }

    /**
     * 
     * @return the sign of the distance
     */
    public double signum() {
        return Math.signum(meters);
    }

    /**
     * Adds two Distances together
     * 
     * @param distance1 the first Distance
     * @param distance2 the second Distance
     * @return the resulting Distance
     */
    public static Distance add(Distance distance1, Distance distance2) {
        return new Distance(distance1.meters + distance2.meters);
    }

    /**
     * Subtracts distance2 from distance1
     * 
     * @param distance1 the first Distance
     * @param distance2 the second Distance
     * @return the resulting Distance
     */
    public static Distance subtract(Distance distance1, Distance distance2) {
        return new Distance(distance1.meters - distance2.meters);
    }

    /**
     * Multiplies a Distance by a number
     * 
     * @param distance the Distance
     * @param number the number to multiply by
     * @return the resulting Distance
     */
    public static Distance multiply(Distance distance, double number) {
        return new Distance(distance.meters * number);
    }

    /**
     * Divides a Distance by a number
     * 
     * @param distance the Distance
     * @param number the number to divide by
     * @return the resulting Distance
     */
    public static Distance divide(Distance distance, double number) {
        return new Distance(distance.meters / number);
    }

    /**
     * Create a Distance that has a value of 0
     * 
     * @return the created Distance
     */
    public static Distance zero() {
        return zero;
    }

    @Override
    public int hashCode() {
        final int prime = 31;
        int result = 1;
        long temp;
        temp = Double.doubleToLongBits(meters);
        result = prime * result + (int) (temp ^ (temp >>> 32));
        return result;
    }

    @Override
    public boolean equals(Object obj) {
        if (this == obj) return true;
        if (obj == null) return false;
        if (getClass() != obj.getClass()) return false;
        Distance other = (Distance) obj;
        if (Double.doubleToLongBits(meters) != Double.doubleToLongBits(other.meters)) return false;
        return true;
    }

    // Create Distance objects from various units
    public static Distance fromMeters(double v) {
        return new Distance(v);
    }

    public static Distance fromKilometers(double v) {
        return new Distance(v * M_PER_KM);
    }

    public static Distance fromCentimeters(double v) {
        return new Distance(v * M_PER_CM);
    }

    public static Distance fromMillimeters(double v) {
        return new Distance(v * M_PER_MM);
    }

    public static Distance fromMicrometers(double v) {
        return new Distance(v * M_PER_UM);
    }

    public static Distance fromNanoMeters(double v) {
        return new Distance(v * M_PER_NM);
    }

    public static Distance fromFeet(double v) {
        return new Distance(v * M_PER_FT);
    }

    public static Distance fromInches(double v) {
        return new Distance(v * M_PER_IN);
    }

    public static Distance fromYards(double v) {
        return new Distance(v * M_PER_YD);
    }

    public static Distance fromMiles(double v) {
        return new Distance(v * M_PER_MI);
    }

    public static Distance fromNauticalMiles(double v) {
        return new Distance(v * M_PER_NAUT_MI);
    }

    // get distance in various units
    public double meters() {
        return meters;
    }

    public double kilometers() {
        return meters * KM_PER_M;
    }

    public double centimeters() {
        return meters * CM_PER_M;
    }

    public double millimeters() {
        return meters * MM_PER_M;
    }

    public double micrometers() {
        return meters * UM_PER_M;
    }

    public double nanometers() {
        return meters * NM_PER_M;
    }

    public double feet() {
        return meters * FT_PER_M;
    }

    public double inches() {
        return meters * IN_PER_M;
    }

    public double yards() {
        return meters * YD_PER_M;
    }

    public double miles() {
        return meters * MI_PER_M;
    }

    public double nauticalMiles() {
        return meters * NAUT_MI_PER_M;
    }
}

Angle | Distance | Time | Velocity | AngularVelocity

Units: seconds, (nano|micro|milli)seconds, minutes, hours, days, weeks, months, years

The Time class stores an amount of Time. It can be created from any of the units above, and you can get the value from it in any of the units above. Once the Time is created, the value cannot be changed.

Time has a few math functions: abs (absolute value), signum (returns the sign), add, subtract, multiply, and divide.

ftc/electronvolts/util/units/Time.java

package ftc.electronvolts.util.units;


/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/5/16
 */

public class Time {
    /*
     * Constants to relate various units to seconds
     * The reciprocal of each constant is calculated once to be used multiple
     * times later
     */
    private static final double S_PER_NS = 1e-9;
    private static final double S_PER_US = 1e-6;
    private static final double S_PER_MS = 0.001;
    private static final double S_PER_MIN = 60;
    private static final double S_PER_HR = 3600;
    private static final double S_PER_DAY = 86400;
    private static final double S_PER_WEEK = 604800;
    private static final double S_PER_MONTH = 2.628e+6;
    private static final double S_PER_YEAR = 3.154e+7;

    private static final double NS_PER_S = 1 / S_PER_NS;
    private static final double US_PER_S = 1 / S_PER_US;
    private static final double MS_PER_S = 1 / S_PER_MS;
    private static final double MIN_PER_S = 1 / S_PER_MIN;
    private static final double HR_PER_S = 1 / S_PER_HR;
    private static final double DAY_PER_S = 1 / S_PER_DAY;
    private static final double WEEK_PER_S = 1 / S_PER_WEEK;
    private static final double MONTH_PER_S = 1 / S_PER_MONTH;
    private static final double YEAR_PER_S = 1 / S_PER_YEAR;

    private static final Time zero = new Time(0);

    // The time in seconds
    private final double seconds;

    /**
     * private constructor to create a Time object from a value in seconds
     * 
     * @param seconds the time in seconds
     */
    private Time(double seconds) {
        this.seconds = seconds;
    }

    /**
     * 
     * @return the absolute value of the time
     */
    public Time abs() {
        return new Time(Math.abs(seconds));
    }

    /**
     * 
     * @return the sign of the angle
     */
    public double signum() {
        return Math.signum(seconds);
    }

    /**
     * Adds two Times together
     * 
     * @param time1 the first Time object
     * @param time2 the second Time object
     * @return the resulting Time
     */
    public static Time add(Time time1, Time time2) {
        return new Time(time1.seconds + time2.seconds);
    }

    /**
     * Subtracts time2 from time1
     * 
     * @param time1 the first Time object
     * @param time2 the second Time object
     * @return the resulting Time
     */
    public static Time subtract(Time time1, Time time2) {
        return new Time(time1.seconds - time2.seconds);
    }

    /**
     * Multiplies a Time object by a number
     * 
     * @param time the Time object
     * @param number the number to multiply by
     * @return the resulting Time
     */
    public static Time multiply(Time time, double number) {
        return new Time(time.seconds * number);
    }

    /**
     * Divides a Time object by a number
     * 
     * @param time the Time object
     * @param number the number to divide by
     * @return the resulting Time
     */
    public static Time divide(Time time, double number) {
        return new Time(time.seconds / number);
    }

    /**
     * Create a Time that has a value of 0
     * 
     * @return the created Time
     */
    public static Time zero() {
        return zero;
    }

    @Override
    public int hashCode() {
        final int prime = 31;
        int result = 1;
        long temp;
        temp = Double.doubleToLongBits(seconds);
        result = prime * result + (int) (temp ^ (temp >>> 32));
        return result;
    }

    @Override
    public boolean equals(Object obj) {
        if (this == obj) return true;
        if (obj == null) return false;
        if (getClass() != obj.getClass()) return false;
        Time other = (Time) obj;
        if (Double.doubleToLongBits(seconds) != Double.doubleToLongBits(other.seconds)) return false;
        return true;
    }

    // create Time objects from various units
    public static Time fromSeconds(double v) {
        return new Time(v);
    }

    public static Time fromNanoseconds(double v) {
        return new Time(v * S_PER_NS);
    }

    public static Time fromMicroseconds(double v) {
        return new Time(v * S_PER_US);
    }

    public static Time fromMilliseconds(double v) {
        return new Time(v * S_PER_MS);
    }

    public static Time fromMinutes(double v) {
        return new Time(v * S_PER_MIN);
    }

    public static Time fromHours(double v) {
        return new Time(v * S_PER_HR);
    }

    public static Time fromDays(double v) {
        return new Time(v * S_PER_DAY);
    }

    public static Time fromWeeks(double v) {
        return new Time(v * S_PER_WEEK);
    }

    public static Time fromMonths(double v) {
        return new Time(v * S_PER_MONTH);
    }

    public static Time fromYears(double v) {
        return new Time(v * S_PER_YEAR);
    }

    // get time in various units
    public double seconds() {
        return seconds;
    }

    public double nanoseconds() {
        return seconds * NS_PER_S;
    }

    public double microseconds() {
        return seconds * US_PER_S;
    }

    public double milliseconds() {
        return seconds * MS_PER_S;
    }

    public double minutes() {
        return seconds * MIN_PER_S;
    }

    public double hours() {
        return seconds * HR_PER_S;
    }

    public double days() {
        return seconds * DAY_PER_S;
    }

    public double weeks() {
        return seconds * WEEK_PER_S;
    }

    public double months() {
        return seconds * MONTH_PER_S;
    }

    public double years() {
        return seconds * YEAR_PER_S;
    }
}

Angle | Distance | Time | Velocity | AngularVelocity

Units: meters, (kilo|centi|milli|micro|nano)meters, feet, inches, yards, miles, nautical miles PER second, (nano|micro|milli)second, minute, hour, day, week, month, year

The Velocity class stores an amount of velocity. It can be created from a Distance class and a Time class, and you can get the value from it in any of the units above. Once the Velocity is created, the value cannot be changed.

Velocity has a few math functions: abs (absolute value), signum (returns the sign), add, subtract, multiply, and divide.

ftc/electronvolts/util/units/Velocity.java

package ftc.electronvolts.util.units;


/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/5/16
 */

public class Velocity {
    private static final Velocity zero = new Velocity(Distance.zero());
    
    private final Distance distance;
    private final Time time;

    /**
     * Distance per Time
     * 
     * @param distance the Distance object
     * @param time the Time object
     */
    public Velocity(Distance distance, Time time) {
        this.distance = distance;
        this.time = time;
    }

    /**
     * Distance per 1 second
     * 
     * @param distance the Distance object
     */
    private Velocity(Distance distance) {
        this(distance, Time.fromSeconds(1));
    }

    /**
     * @return a Velocity with a value of 0
     */
    public static Velocity zero() {
        return zero;
    }

    public Distance getDistance(Time time) {
        // distance = velocity * time
        return Distance.fromMeters(metersPerSecond() * time.seconds());
    }

    public Time getTime(Distance distance) {
        // time = distance / velocity
        return Time.fromSeconds(distance.meters() / metersPerSecond());
    }

    public AngularVelocity getAngularVelocity(Distance radius) {
        //angular velocity = velocity / radius
        return new AngularVelocity(Angle.fromRadians(metersPerSecond() / radius.meters()), Time.fromSeconds(1));
    }

    public Velocity abs() {
        return new Velocity(distance.abs(), time.abs());
    }

    public double signum() {
        return Math.signum(metersPerSecond());
    }

    /**
     * Adds two Velocities together
     * 
     * @param velocity1 the first Velocity
     * @param velocity2 the second Velocity
     * @return the resulting Velocity
     */
    public static Velocity add(Velocity velocity1, Velocity velocity2) {
        return new Velocity(Distance.fromMeters(velocity1.metersPerSecond() + velocity2.metersPerSecond()));
    }

    /**
     * Subtracts velocity2 from velocity1
     * 
     * @param velocity1 the first Velocity
     * @param velocity2 the second Velocity
     * @return the resulting Velocity
     */
    public static Velocity subtract(Velocity velocity1, Velocity velocity2) {
        return new Velocity(Distance.fromMeters(velocity1.metersPerSecond() - velocity2.metersPerSecond()));
    }

    /**
     * Multiplies a Velocity by a number
     * 
     * @param velocity the Velocity
     * @param number the number to multiply by
     * @return the resulting Velocity
     */
    public static Velocity multiply(Velocity velocity, double number) {
        return new Velocity(Distance.fromMeters(velocity.metersPerSecond() * number));
    }

    /**
     * Divides a Velocity by a number
     * 
     * @param velocity the Velocity
     * @param number the number to divide by
     * @return the resulting Velocity
     */
    public static Velocity divide(Velocity velocity, double number) {
        return new Velocity(Distance.fromMeters(velocity.metersPerSecond() * number));
    }

    @Override
    public int hashCode() {
        final int prime = 31;
        int result = 1;
        long temp;
        temp = Double.doubleToLongBits(metersPerSecond());
        result = prime * result + (int) (temp ^ (temp >>> 32));
        return result;
    }

    @Override
    public boolean equals(Object obj) {
        if (this == obj) return true;
        if (obj == null) return false;
        if (getClass() != obj.getClass()) return false;
        Velocity other = (Velocity) obj;
        if (Double.doubleToLongBits(metersPerSecond()) != Double.doubleToLongBits(other.metersPerSecond())) return false;
        return true;
    }

    // get velocity in various units
    public double metersPerNanosecond() {
        return distance.meters() / time.nanoseconds();
    }

    public double metersPerMicrosecond() {
        return distance.meters() / time.microseconds();
    }

    public double metersPerMillisecond() {
        return distance.meters() / time.milliseconds();
    }

    public double metersPerSecond() {
        return distance.meters() / time.seconds();
    }

    public double metersPerMinute() {
        return distance.meters() / time.minutes();
    }

    public double metersPerHour() {
        return distance.meters() / time.hours();
    }

    public double metersPerDay() {
        return distance.meters() / time.days();
    }

    public double metersPerWeek() {
        return distance.meters() / time.weeks();
    }

    public double metersPerMonth() {
        return distance.meters() / time.months();
    }

    public double metersPerYear() {
        return distance.meters() / time.years();
    }

    public double kilometersPerNanosecond() {
        return distance.kilometers() / time.nanoseconds();
    }

    public double kilometersPerMicrosecond() {
        return distance.kilometers() / time.microseconds();
    }

    public double kilometersPerMillisecond() {
        return distance.kilometers() / time.milliseconds();
    }

    public double kilometersPerSecond() {
        return distance.kilometers() / time.seconds();
    }

    public double kilometersPerMinute() {
        return distance.kilometers() / time.minutes();
    }

    public double kilometersPerHour() {
        return distance.kilometers() / time.hours();
    }

    public double kilometersPerDay() {
        return distance.kilometers() / time.days();
    }

    public double kilometersPerWeek() {
        return distance.kilometers() / time.weeks();
    }

    public double kilometersPerMonth() {
        return distance.kilometers() / time.months();
    }

    public double kilometersPerYear() {
        return distance.kilometers() / time.years();
    }

    public double centimetersPerNanosecond() {
        return distance.centimeters() / time.nanoseconds();
    }

    public double centimetersPerMicrosecond() {
        return distance.centimeters() / time.microseconds();
    }

    public double centimetersPerMillisecond() {
        return distance.centimeters() / time.milliseconds();
    }

    public double centimetersPerSecond() {
        return distance.centimeters() / time.seconds();
    }

    public double centimetersPerMinute() {
        return distance.centimeters() / time.minutes();
    }

    public double centimetersPerHour() {
        return distance.centimeters() / time.hours();
    }

    public double centimetersPerDay() {
        return distance.centimeters() / time.days();
    }

    public double centimetersPerWeek() {
        return distance.centimeters() / time.weeks();
    }

    public double centimetersPerMonth() {
        return distance.centimeters() / time.months();
    }

    public double centimetersPerYear() {
        return distance.centimeters() / time.years();
    }

    public double millimetersPerNanosecond() {
        return distance.millimeters() / time.nanoseconds();
    }

    public double millimetersPerMicrosecond() {
        return distance.millimeters() / time.microseconds();
    }

    public double millimetersPerMillisecond() {
        return distance.millimeters() / time.milliseconds();
    }

    public double millimetersPerSecond() {
        return distance.millimeters() / time.seconds();
    }

    public double millimetersPerMinute() {
        return distance.millimeters() / time.minutes();
    }

    public double millimetersPerHour() {
        return distance.millimeters() / time.hours();
    }

    public double millimetersPerDay() {
        return distance.millimeters() / time.days();
    }

    public double millimetersPerWeek() {
        return distance.millimeters() / time.weeks();
    }

    public double millimetersPerMonth() {
        return distance.millimeters() / time.months();
    }

    public double millimetersPerYear() {
        return distance.millimeters() / time.years();
    }

    public double micrometersPerNanosecond() {
        return distance.micrometers() / time.nanoseconds();
    }

    public double micrometersPerMicrosecond() {
        return distance.micrometers() / time.microseconds();
    }

    public double micrometersPerMillisecond() {
        return distance.micrometers() / time.milliseconds();
    }

    public double micrometersPerSecond() {
        return distance.micrometers() / time.seconds();
    }

    public double micrometersPerMinute() {
        return distance.micrometers() / time.minutes();
    }

    public double micrometersPerHour() {
        return distance.micrometers() / time.hours();
    }

    public double micrometersPerDay() {
        return distance.micrometers() / time.days();
    }

    public double micrometersPerWeek() {
        return distance.micrometers() / time.weeks();
    }

    public double micrometersPerMonth() {
        return distance.micrometers() / time.months();
    }

    public double micrometersPerYear() {
        return distance.micrometers() / time.years();
    }

    public double nanometersPerNanosecond() {
        return distance.nanometers() / time.nanoseconds();
    }

    public double nanometersPerMicrosecond() {
        return distance.nanometers() / time.microseconds();
    }

    public double nanometersPerMillisecond() {
        return distance.nanometers() / time.milliseconds();
    }

    public double nanometersPerSecond() {
        return distance.nanometers() / time.seconds();
    }

    public double nanometersPerMinute() {
        return distance.nanometers() / time.minutes();
    }

    public double nanometersPerHour() {
        return distance.nanometers() / time.hours();
    }

    public double nanometersPerDay() {
        return distance.nanometers() / time.days();
    }

    public double nanometersPerWeek() {
        return distance.nanometers() / time.weeks();
    }

    public double nanometersPerMonth() {
        return distance.nanometers() / time.months();
    }

    public double nanometersPerYear() {
        return distance.nanometers() / time.years();
    }

    public double feetPerNanosecond() {
        return distance.feet() / time.nanoseconds();
    }

    public double feetPerMicrosecond() {
        return distance.feet() / time.microseconds();
    }

    public double feetPerMillisecond() {
        return distance.feet() / time.milliseconds();
    }

    public double feetPerSecond() {
        return distance.feet() / time.seconds();
    }

    public double feetPerMinunte() {
        return distance.feet() / time.minutes();
    }

    public double feetPerHour() {
        return distance.feet() / time.hours();
    }

    public double feetPerDay() {
        return distance.feet() / time.days();
    }

    public double feetPerWeek() {
        return distance.feet() / time.weeks();
    }

    public double feetPerMonth() {
        return distance.feet() / time.months();
    }

    public double feetPerYear() {
        return distance.feet() / time.years();
    }

    public double inchesPerNanosecond() {
        return distance.inches() / time.nanoseconds();
    }

    public double inchesPerMicrosecond() {
        return distance.inches() / time.microseconds();
    }

    public double inchesPerMillisecond() {
        return distance.inches() / time.milliseconds();
    }

    public double inchesPerSecond() {
        return distance.inches() / time.seconds();
    }

    public double inchesPerMinute() {
        return distance.inches() / time.minutes();
    }

    public double inchesPerHour() {
        return distance.inches() / time.hours();
    }

    public double inchesPerDay() {
        return distance.inches() / time.days();
    }

    public double inchesPerWeek() {
        return distance.inches() / time.weeks();
    }

    public double inchesPerMonth() {
        return distance.inches() / time.months();
    }

    public double inchesPerYear() {
        return distance.inches() / time.years();
    }

    public double yardsPerNanosecond() {
        return distance.yards() / time.nanoseconds();
    }

    public double yardsPerMicrosecond() {
        return distance.yards() / time.microseconds();
    }

    public double yardsPerMillisecond() {
        return distance.yards() / time.milliseconds();
    }

    public double yardsPerSecond() {
        return distance.yards() / time.seconds();
    }

    public double yardsPerMinute() {
        return distance.yards() / time.minutes();
    }

    public double yardsPerHour() {
        return distance.yards() / time.hours();
    }

    public double yardsPerDay() {
        return distance.yards() / time.days();
    }

    public double yardsPerWeek() {
        return distance.yards() / time.weeks();
    }

    public double yardsPerMonth() {
        return distance.yards() / time.months();
    }

    public double yardsPerYear() {
        return distance.yards() / time.years();
    }

    public double milesPerNanosecond() {
        return distance.miles() / time.nanoseconds();
    }

    public double milesPerMicrosecond() {
        return distance.miles() / time.microseconds();
    }

    public double milesPerMillisecond() {
        return distance.miles() / time.milliseconds();
    }

    public double milesPerSecond() {
        return distance.miles() / time.seconds();
    }

    public double milesPerMinute() {
        return distance.miles() / time.minutes();
    }

    public double milesPerHour() {
        return distance.miles() / time.hours();
    }

    public double milesPerDay() {
        return distance.miles() / time.days();
    }

    public double milesPerWeek() {
        return distance.miles() / time.weeks();
    }

    public double milesPerMonth() {
        return distance.miles() / time.months();
    }

    public double milesPerYear() {
        return distance.miles() / time.years();
    }

    public double nauticalMilesPerNanosecond() {
        return distance.nauticalMiles() / time.nanoseconds();
    }

    public double nauticalMilesPerMicrosecond() {
        return distance.nauticalMiles() / time.microseconds();
    }

    public double nauticalMilesPerMillisecond() {
        return distance.nauticalMiles() / time.milliseconds();
    }

    public double nauticalMilesPerSecond() {
        return distance.nauticalMiles() / time.seconds();
    }

    public double nauticalMilesPerMinute() {
        return distance.nauticalMiles() / time.minutes();
    }

    public double nauticalMilesPerHour() {
        return distance.nauticalMiles() / time.hours();
    }

    public double nauticalMilesPerDay() {
        return distance.nauticalMiles() / time.days();
    }

    public double nauticalMilesPerWeek() {
        return distance.nauticalMiles() / time.weeks();
    }

    public double nauticalMilesPerMonth() {
        return distance.nauticalMiles() / time.months();
    }

    public double nauticalMilesPerYear() {
        return distance.nauticalMiles() / time.years();
    }
}

Angle | Distance | Time | Velocity | AngularVelocity

Units: radians, degrees, rotations PER second, (nano|micro|milli)second, minute, hour, day, week, month, year

The AngularVelocity class stores an amount of angular velocity. It can be created from an Angle class and a Time class, and you can get the value from it in any of the units above. Once the AngularVelocity is created, the value cannot be changed.

AngularVelocity has a few math functions: abs (absolute value), signum (returns the sign), add, subtract, multiply, and divide.

ftc/electronvolts/util/units/AngularVelocity.java

package ftc.electronvolts.util.units;


/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 11/16/16
 */

public class AngularVelocity {
    private static final AngularVelocity zero = new AngularVelocity(Angle.zero());
    
    private final Angle angle;
    private final Time time;

    /**
     * Angle per Time
     * 
     * @param angle the Angle object
     * @param time the Time object
     */
    public AngularVelocity(Angle angle, Time time) {
        this.angle = angle;
        this.time = time;
    }

    /**
     * Angle per 1 second
     * 
     * @param angle the Angle object
     */
    private AngularVelocity(Angle angle) {
        this(angle, Time.fromSeconds(1));
    }

    public static AngularVelocity zero() {
        return zero;
    }

    public Angle getAngle(Time time) {
        // angle = angular velocity * time
        return Angle.fromRadians(radiansPerSecond() * time.seconds());
    }

    public Time getTime(Angle angle) {
        // time = angle / angular velocity
        return Time.fromSeconds(angle.radians() / radiansPerSecond());
    }

    public Velocity getVelocity(Distance radius) {
        //velocity = radius * angular velocity
        return new Velocity(Distance.fromMeters(radius.meters() * radiansPerSecond()), Time.fromSeconds(1));
    }

    public AngularVelocity abs() {
        return new AngularVelocity(angle.abs(), time.abs());
    }

    public double signum() {
        return Math.signum(radiansPerSecond());
    }

    /**
     * Adds two AngularVelocity objects together
     * 
     * @param angularVelocity1 the first AngularVelocity
     * @param angularVelocity2 the second AngularVelocity
     * @return the resulting AngularVelocity
     */
    public static AngularVelocity add(AngularVelocity angularVelocity1, AngularVelocity angularVelocity2) {
        return new AngularVelocity(Angle.fromRadians(angularVelocity1.radiansPerSecond() + angularVelocity2.radiansPerSecond()));
    }

    /**
     * Subtracts velocity2 from velocity1
     * 
     * @param angularVelocity1 the first AngularVelocity
     * @param angularVelocity2 the second AngularVelocity
     * @return the resulting AngularVelocity
     */
    public static AngularVelocity subtract(AngularVelocity angularVelocity1, AngularVelocity angularVelocity2) {
        return new AngularVelocity(Angle.fromRadians(angularVelocity1.radiansPerSecond() - angularVelocity2.radiansPerSecond()));
    }

    /**
     * Multiplies an AngularVelocity by a number
     * 
     * @param angularVelocity the AngularVelocity
     * @param number the number to multiply by
     * @return the resulting AngularVelocity
     */
    public static AngularVelocity multiply(AngularVelocity angularVelocity, double number) {
        return new AngularVelocity(Angle.fromRadians(angularVelocity.radiansPerSecond() * number));
    }

    /**
     * Divides an AngularVelocity by a number
     * 
     * @param angularVelocity the AngularVelocity
     * @param number the number to divide by
     * @return the resulting AngularVelocity
     */
    public static AngularVelocity divide(AngularVelocity angularVelocity, double number) {
        return new AngularVelocity(Angle.fromRadians(angularVelocity.radiansPerSecond() * number));
    }

    @Override
    public int hashCode() {
        final int prime = 31;
        int result = 1;
        long temp;
        temp = Double.doubleToLongBits(radiansPerSecond());
        result = prime * result + (int) (temp ^ (temp >>> 32));
        return result;
    }

    @Override
    public boolean equals(Object obj) {
        if (this == obj) return true;
        if (obj == null) return false;
        if (getClass() != obj.getClass()) return false;
        AngularVelocity other = (AngularVelocity) obj;
        if (Double.doubleToLongBits(radiansPerSecond()) != Double.doubleToLongBits(other.radiansPerSecond())) return false;
        return true;
    }

    // get angular velocity in various units
    public double radiansPerNanosecond() {
        return angle.radians() / time.nanoseconds();
    }

    public double radiansPerMicrosecond() {
        return angle.radians() / time.microseconds();
    }

    public double radiansPerMillisecond() {
        return angle.radians() / time.milliseconds();
    }

    public double radiansPerSecond() {
        return angle.radians() / time.seconds();
    }

    public double radiansPerMinute() {
        return angle.radians() / time.minutes();
    }

    public double radiansPerHour() {
        return angle.radians() / time.hours();
    }

    public double radiansPerDay() {
        return angle.radians() / time.days();
    }

    public double radiansPerWeek() {
        return angle.radians() / time.weeks();
    }

    public double radiansPerMonth() {
        return angle.radians() / time.months();
    }

    public double radiansPerYear() {
        return angle.radians() / time.years();
    }

    public double degreesPerNanosecond() {
        return angle.degrees() / time.nanoseconds();
    }

    public double degreesPerMicrosecond() {
        return angle.degrees() / time.microseconds();
    }

    public double degreesPerMillisecond() {
        return angle.degrees() / time.milliseconds();
    }

    public double degreesPerSecond() {
        return angle.degrees() / time.seconds();
    }

    public double degreesPerMinute() {
        return angle.degrees() / time.minutes();
    }

    public double degreesPerHour() {
        return angle.degrees() / time.hours();
    }

    public double degreesPerDay() {
        return angle.degrees() / time.days();
    }

    public double degreesPerWeek() {
        return angle.degrees() / time.weeks();
    }

    public double degreesPerMonth() {
        return angle.degrees() / time.months();
    }

    public double degreesPerYear() {
        return angle.degrees() / time.years();
    }

    public double rotationsPerNanosecond() {
        return angle.rotations() / time.nanoseconds();
    }

    public double rotationsPerMicrosecond() {
        return angle.rotations() / time.microseconds();
    }

    public double rotationsPerMillisecond() {
        return angle.rotations() / time.milliseconds();
    }

    public double rotationsPerSecond() {
        return angle.rotations() / time.seconds();
    }

    public double rotationsPerMinute() {
        return angle.rotations() / time.minutes();
    }

    public double rotationsPerHour() {
        return angle.rotations() / time.hours();
    }

    public double rotationsPerDay() {
        return angle.rotations() / time.days();
    }

    public double rotationsPerWeek() {
        return angle.rotations() / time.weeks();
    }

    public double rotationsPerMonth() {
        return angle.rotations() / time.months();
    }

    public double rotationsPerYear() {
        return angle.rotations() / time.years();
    }

}

Vector2D | Vector3D

Vector2D stores a mathematical vector in 2 dimensions. You can create it from cartesian (x, y) with new Vector2D(x, y) or polar (r, θ) with Vector2D.fromPolar2D(r, theta), where "theta" is an Angle object.

Vector2D has 4 getters:

• double getX()
• double getY()
• double getLength()
• Angle getDirection()

A static method to compute the dot product of two Vector2D objects.

public static double dotProduct(Vector2D v1, Vector2D v2)

And a static method to find the signed separation between vectors.

public static Angle signedAngularSeparation(Vector2D ref, Vector2D vector)

signedAngularSeparation can be very useful when dealing with a gyro sensor that can wrap around from 360 to 0. It returns an Angle object.

ftc/electronvolts/util/Vector2D.java

package ftc.electronvolts.util;

import ftc.electronvolts.util.units.Angle;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/1/16
 */

public class Vector2D {
    /*
     * the 2 components of the vector
     */
    private final double x;
    private final double y;
    
    /*
     * the polar coordinates
     */
    private final double l;
    private final Angle theta;

    /**
     * create a vector using polar coordinates
     *
     * @param magnitude the magnitude of the 2-D vector
     * @param theta the direction of the 2-D vector
     * @return the created vector
     */
    public Vector2D(double magnitude, Angle theta) {
        double thetaRads = theta.radians();
        this.x = magnitude * Math.cos(thetaRads);
        this.y = magnitude * Math.sin(thetaRads);
        
        this.l = magnitude;
        this.theta = theta;
    }

    /**
     * create a vector using x and y
     *
     * @param x x component
     * @param y y component
     */
    public Vector2D(double x, double y) {
        this.x = x;
        this.y = y;
        
        //Pythagorean theorem
        this.l = Math.sqrt(x * x + y * y);
        this.theta = Angle.fromRadians(Math.atan2(y, x));
    }

    /**
     * @return the x component of the vector
     */
    public double getX() {
        return x;
    }

    /**
     * @return the y component of the vector
     */
    public double getY() {
        return y;
    }

    /**
     * @return the length or magnitude of the vector
     */
    public double getLength() {
        return l;
    }

    /**
     * @return a new vector that is normalized (length = 1)
     */
    public Vector2D normalized() {
        return new Vector2D(x / l, y / l);
    }

    /**
     * The order does not matter for the dot product
     *
     * @param v1 one vector
     * @param v2 another vector
     * @return the dot product of the two vectors
     */
    public static double dotProduct(Vector2D v1, Vector2D v2) {
        return v1.x * v2.x + v1.y * v2.y;
    }

    /**
     * @return the direction of the vector
     */
    public Angle getDirection() {
        return theta;
    }
    

    private final static double closeToZero = 1.0e-3;

    /**
     * This helps when using a gyro sensor or any type of 360 degree rotation
     * mechanism
     *
     * @param ref the reference direction, assumed to have no z component
     * @param vector the vector to be measured against the reference to find the
     *            angular separation, also assumed to have no z component
     * @return angular separation between -pi and pi. If vector is to the right
     *         of reference, then it is positive.
     */
    public static Angle signedAngularSeparation(Vector2D ref, Vector2D vector) {
        Vector3D ref3D = Vector3D.from2D(ref);
        Vector3D vector3D = Vector3D.from2D(vector);
        
        Vector3D cross = Vector3D.crossProduct(ref3D, vector3D);
        // If the vectors are too closely aligned, then return zero for
        // separation.
        if (Math.abs(cross.getZ()) < closeToZero) {
            return Angle.zero();
        }
        // To get the angle:
        // a dot b = a * b * cos(angle)
        // so angle = acos[ (a dot b) / (a * b) ]
        // Make sure a * b is not too close to zero 0
        double lengths = ref3D.getLength() * vector3D.getLength();
        if (lengths < closeToZero) {
            // this is really an error, but to keep the robot from crashing,
            // just return 0
            return Angle.zero();
        }
        double dot = Vector3D.dotProduct(ref3D, vector3D);

        return Angle.fromRadians(Math.signum(cross.getZ()) * Math.acos(dot / lengths));
    }

    @Override
    public int hashCode() {
        final int prime = 31;
        int result = 1;
        long temp;
        temp = Double.doubleToLongBits(x);
        result = prime * result + (int) (temp ^ (temp >>> 32));
        temp = Double.doubleToLongBits(y);
        result = prime * result + (int) (temp ^ (temp >>> 32));
        return result;
    }

    @Override
    public boolean equals(Object obj) {
        if (this == obj) return true;
        if (obj == null) return false;
        if (getClass() != obj.getClass()) return false;
        Vector2D other = (Vector2D) obj;
        if (Double.doubleToLongBits(x) != Double.doubleToLongBits(other.x)) return false;
        if (Double.doubleToLongBits(y) != Double.doubleToLongBits(other.y)) return false;
        return true;
    }

    @Override
    public String toString() {
        return "(" + x + ", " + y + ")";
    }
}

Vector2D | Vector3D

Vector3D stores a mathematical vector in 3 dimensions. You can create it from cartesian (x, y, z) with new Vector3D(x, y, z), spherical (r, θ, φ) using Vector3D(magnitude, theta, phi), or polar (r, θ) using only 2 dimensions with Vector2D.fromPolar2D(r, theta), where "theta" is an Angle object.

Vector3D has 6 getters:

• double getX()
• double getY()
• double getZ()
• double getLength()
• AnglegetTheta()
• AnglegetPhi()

And static methods to compute the dot product, cross product, and separation of two Vector3D objects.

public static double dotProduct(Vector3D v1, Vector3D v2)
public static Vector3D crossProduct(Vector3D v1, Vector3D v2)
public static Angle angularSeparation(Vector3D ref, Vector3D vector)

ftc/electronvolts/util/Vector3D.java

package ftc.electronvolts.util;

import ftc.electronvolts.util.units.Angle;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 9/30/16
 */
public class Vector3D {
    /*
     * the 3 components of the vector
     */
    private final double x;
    private final double y;
    private final double z;

    /*
     * the spherical coordinates
     */
    private final double l;
    private final Angle theta;
    private final Angle phi;

    /**
     * create a vector using polar coordinates with z = 0
     *
     * @param magnitude the magnitude of the Vector2D
     * @param theta the direction of the Vector2D
     * @return the created Vector3D
     */
    public static Vector3D fromPolar2D(double magnitude, Angle theta) {
        return from2D(new Vector2D(magnitude, theta));
    }

    /**
     * create a vector from a Vector2D with z = 0
     * 
     * @param vector2D the 2D vector to use
     * @return the created Vector3D
     */
    public static Vector3D from2D(Vector2D vector2D) {
        return new Vector3D(vector2D.getX(), vector2D.getY(), 0);
    }

    /**
     * Create a vector using spherical coordinates
     * 
     * @param magnitude the magnitude of the 3D vector
     * @param theta the direction in the x-y plane
     * @param phi the z direction
     * @return
     */
    public Vector3D(double magnitude, Angle theta, Angle phi) {
        double thetaRads = theta.radians();
        double phiRads = phi.radians();

        // http://mathinsight.org/spherical_coordinates
        // x = ρ sinϕ cosθ
        // y = ρ sinϕ sinθ
        // z = ρ cosϕ
        
        this.x = magnitude * Math.sin(phiRads) * Math.cos(thetaRads);
        this.y = magnitude * Math.sin(phiRads) * Math.sin(thetaRads);
        this.z = magnitude * Math.cos(phiRads);
        
        this.l = magnitude;
        this.theta = theta;
        this.phi = phi;
    }

    /**
     * create a vector using x, y, and z
     *
     * @param x x component
     * @param y y component
     * @param z z component
     */
    public Vector3D(double x, double y, double z) {
        this.x = x;
        this.y = y;
        this.z = z;
        
        //Pythagorean theorem
        this.l = Math.sqrt(x * x + y * y + z * z);
        
        //compute spherical coordinates
        phi = Angle.fromRadians(Math.acos(z / l));
        theta = Angle.fromRadians(Math.atan2(y, x));
    }


    /**
     * @return the x component of the vector
     */
    public double getX() {
        return x;
    }

    /**
     * @return the y component of the vector
     */
    public double getY() {
        return y;
    }

    /**
     * @return the z component of the vector
     */
    public double getZ() {
        return z;
    }

    /**
     * @return the length or magnitude of the vector
     */
    public double getLength() {
        return l;
    }

    /**
     * @return the z direction
     */
    public Angle getPhi() {
        return phi;
    }

    /**
     * @return the x-y direction
     */
    public Angle getTheta() {
        return theta;
    }

    /**
     * @return a new vector that is normalized (length = 1)
     */
    public Vector3D normalized() {
        return new Vector3D(x / l, y / l, z / l);
    }

    /**
     * Order matters for the cross product
     *
     * @param v1 the first vector
     * @param v2 the second vector
     * @return the first vector crossed with the second vector
     */
    public static Vector3D crossProduct(Vector3D v1, Vector3D v2) {
        double x = v1.y * v2.z - v1.z * v2.y;
        double y = v1.z * v2.x - v1.x * v2.z;
        double z = v1.x * v2.y - v1.y * v2.x;
        return new Vector3D(x, y, z);
    }

    /**
     * The order does not matter for the dot product
     *
     * @param v1 one vector
     * @param v2 another vector
     * @return the dot product of the two vectors
     */
    public static double dotProduct(Vector3D v1, Vector3D v2) {
        return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
    }
    
    /**
     * 
     * @param v1 one vector
     * @param v2 another vector
     * @return the angle between the two vectors
     */
    public static Angle angularSeparation(Vector3D v1, Vector3D v2){
        //             a dot b
        //cos theta = ---------
        //            |a| * |b|
        return Angle.fromRadians(Math.acos(dotProduct(v1, v2) / (v1.l * v2.l)));
    }

    @Override
    public int hashCode() {
        final int prime = 31;
        int result = 1;
        long temp;
        temp = Double.doubleToLongBits(x);
        result = prime * result + (int) (temp ^ (temp >>> 32));
        temp = Double.doubleToLongBits(y);
        result = prime * result + (int) (temp ^ (temp >>> 32));
        temp = Double.doubleToLongBits(z);
        result = prime * result + (int) (temp ^ (temp >>> 32));
        return result;
    }

    @Override
    public boolean equals(Object obj) {
        if (this == obj) return true;
        if (obj == null) return false;
        if (getClass() != obj.getClass()) return false;
        Vector3D other = (Vector3D) obj;
        if (Double.doubleToLongBits(x) != Double.doubleToLongBits(other.x)) return false;
        if (Double.doubleToLongBits(y) != Double.doubleToLongBits(other.y)) return false;
        if (Double.doubleToLongBits(z) != Double.doubleToLongBits(other.z)) return false;
        return true;
    }

    @Override
    public String toString() {
        return "(" + x + ", " + y + ", " + z + ")";
    }

}

Are the values from your methods out of range? Have you ever wanted to limit a number? The Utility(TM) class is for you! Limit a value before sending it to a motor or servo with motorLimit(v) and servoLimit(v)! Limit a value to ±N with mirrorLimit(v, N)! Or go plain and simple with a good old limit(v, A, B) function. The possibilities are limitless!

But wait -- there's more! Now joining us are some new methods! These methods are great for joining arrays OR lists of ANY type! Just pass in the array or list and a delimiter and you are good to go. Don't wait to sign up -- if you join today, you get a FREE 2-week trial -- for FREE!

ftc/electronvolts/util/Utility.java

package ftc.electronvolts.util;

import java.util.Collection;

/**
 * This file was made by the electronVolts, FTC team 7393 Date Created: 2/17/16
 */
public class Utility {
    /**
     * limits the value to be between a and b
     *
     * @param input the value to be limited
     * @param a one end of the range
     * @param b the other end of the range
     * @return the input limited to the range between a and b
     */
    public static double limit(double input, double a, double b) {
        if (a == b) return a; // if the ends of the range are equal

        // set min and max to a and b, making sure that min < max
        double min = a, max = b;
        if (a > b) {
            min = b;
            max = a;
        }

        // limit the input to be min < input < max
        if (input > max) return max;
        if (input < min) return min;
        return input;
    }

    /**
     * a limit function where min = -max
     *
     * @param input the value to be limited
     * @param max the max absolute value
     * @return the input limited to be between -max to max
     */
    public static double mirrorLimit(double input, double max) {
        return limit(input, -max, max);
    }

    /**
     * Limiting function for motor power
     *
     * @param input the value to be limited
     * @return the input limited to the range from -1 to 1
     */
    public static double motorLimit(double input) {
        return mirrorLimit(input, 1);
    }

    /**
     * Limiting function for servo positions
     *
     * @param input the value to be limited
     * @return the input limited to the range from 0 to 1
     */
    public static double servoLimit(double input) {
        return limit(input, 0, 1);
    }

    /**
     * Join a list of objects with a separator
     * 
     * @param list the list of values
     * @param separator the String to put between the values
     * @return a String of the joined items
     */
    public static <T> String join(Collection<T> list, String separator) {
        StringBuilder sb = new StringBuilder();
        boolean first = true;
        for (T item : list) {
            if (first) {
                first = false;
            } else {
                sb.append(separator);
            }
            sb.append(item);
        }
        return sb.toString();
    }

    /**
     * Join an array of booleans with a separator
     * 
     * @param array the array of values
     * @param separator the String to put between the values
     * @return a String of the joined items
     */
    public static String join(boolean[] array, String separator) {
        StringBuilder sb = new StringBuilder();
        boolean first = true;
        for (boolean item : array) {
            if (first) {
                first = false;
            } else {
                sb.append(separator);
            }
            sb.append(item);
        }
        return sb.toString();
    }

    /**
     * Join an array of bytes with a separator
     * 
     * @param array the array of values
     * @param separator the String to put between the values
     * @return a String of the joined items
     */
    public static String join(byte[] array, String separator) {
        StringBuilder sb = new StringBuilder();
        boolean first = true;
        for (byte item : array) {
            if (first) {
                first = false;
            } else {
                sb.append(separator);
            }
            sb.append(item);
        }
        return sb.toString();
    }

    /**
     * Join an array of chars with a separator
     * 
     * @param array the array of values
     * @param separator the String to put between the values
     * @return a String of the joined items
     */
    public static String join(char[] array, String separator) {
        StringBuilder sb = new StringBuilder();
        boolean first = true;
        for (char item : array) {
            if (first) {
                first = false;
            } else {
                sb.append(separator);
            }
            sb.append(item);
        }
        return sb.toString();
    }

    /**
     * Join an array of shorts with a separator
     * 
     * @param array the array of values
     * @param separator the String to put between the values
     * @return a String of the joined items
     */
    public static String join(short[] array, String separator) {
        StringBuilder sb = new StringBuilder();
        boolean first = true;
        for (short item : array) {
            if (first) {
                first = false;
            } else {
                sb.append(separator);
            }
            sb.append(item);
        }
        return sb.toString();
    }

    /**
     * Join an array of ints with a separator
     * 
     * @param array the array of values
     * @param separator the String to put between the values
     * @return a String of the joined items
     */
    public static String join(int[] array, String separator) {
        StringBuilder sb = new StringBuilder();
        boolean first = true;
        for (int item : array) {
            if (first) {
                first = false;
            } else {
                sb.append(separator);
            }
            sb.append(item);
        }
        return sb.toString();
    }

    /**
     * Join an array of longs with a separator
     * 
     * @param array the array of values
     * @param separator the String to put between the values
     * @return a String of the joined items
     */
    public static String join(long[] array, String separator) {
        StringBuilder sb = new StringBuilder();
        boolean first = true;
        for (long item : array) {
            if (first) {
                first = false;
            } else {
                sb.append(separator);
            }
            sb.append(item);
        }
        return sb.toString();
    }

    /**
     * Join an array of floats with a separator
     * 
     * @param array the array of values
     * @param separator the String to put between the values
     * @return a String of the joined items
     */
    public static String join(float[] array, String separator) {
        StringBuilder sb = new StringBuilder();
        boolean first = true;
        for (float item : array) {
            if (first) {
                first = false;
            } else {
                sb.append(separator);
            }
            sb.append(item);
        }
        return sb.toString();
    }

    /**
     * Join an array of doubles with a separator
     * 
     * @param array the array of values
     * @param separator the String to put between the values
     * @return a String of the joined items
     */
    public static String join(double[] array, String separator) {
        StringBuilder sb = new StringBuilder();
        boolean first = true;
        for (double item : array) {
            if (first) {
                first = false;
            } else {
                sb.append(separator);
            }
            sb.append(item);
        }
        return sb.toString();
    }

}

In order for all the classes to have easy access to the telemetry object, we decided to make it global. It is set inside AbstractOp so any opmode that extends AbstractOp automatically sets it up. If you use normal opmodes, you can still set it up with Telem.telemetry = telemetry;

To use it, you can add a static import:

import static ftc.evlib.driverstation.Telem.telemetry;

Then access it later in the class:

telemtry.addData("Test", 3);

//this displays whether or not the gyro is calibrated on the driver phone
Telem.displayGyroIsCalibrated(gyro);

ftc/evlib/driverstation/Telem.java

package ftc.evlib.driverstation;

import com.qualcomm.robotcore.hardware.GyroSensor;

import org.firstinspires.ftc.robotcore.external.Telemetry;

import ftc.electronvolts.util.ResultReceiver;
import ftc.evlib.Fake;
import ftc.evlib.vision.framegrabber.VuforiaFrameFeeder;
import ftc.evlib.vision.processors.BeaconColorResult;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 9/12/16
 *
 * This class gives everything access to the telemetry
 * Put this in your opmode:
 *
 * Telem.telemetry = telemetry;
 *
 * To set the telemetry variable for anything to use.
 *
 * @see Telemetry
 */
public class Telem {
    //this will fail silently and move on if you write to it before it is set by the opmode
    public static Telemetry telemetry = Fake.TELEMETRY;

    //this will cause errors if you write to it before it is set by the opmode
//    public static Telemetry telemetry = null;

    /**
     * Display whether or not a ResultReceiver is ready on the driver station telemetry
     *
     * @param receiver the ResultReceiver
     * @param caption  the caption of the telemetry item
     * @param ready    the value of the telemetry item if ready
     * @param notReady the value of the telemetry item if not ready
     */
    public static void displayReceiverReadiness(ResultReceiver receiver, String caption, String ready, String notReady) {
        if (receiver.isReady()) {
            telemetry.addData(caption, ready);
        } else {
            telemetry.addData(caption, notReady);
        }
    }

    /**
     * Display whether or not a Vuforia ResultReceiver is ready on the driver station telemetry
     *
     * @param receiver the ResultReceiver of the VuforiaFrameFeeder type
     */
    public static void displayVuforiaReadiness(ResultReceiver<VuforiaFrameFeeder> receiver) {
        displayReceiverReadiness(receiver, "vuforia", "ready", "NOT INITIALIZED!!!");
    }

    /**
     * Display a BeaconColorResult on the telemetry
     *
     * @param receiver the ResultReceiver that contains the BeaconColorResult
     */
    public static void displayBeaconColorResult(ResultReceiver<BeaconColorResult> receiver) {
        if (receiver.isReady()) {
            BeaconColorResult result = receiver.getValue();
            telemetry.addData("leftColor", result.getLeftColor());
            telemetry.addData("rightColor", result.getRightColor());
        } else {
            telemetry.addData("receiver not ready", "");
        }
    }


    /**
     * Display a BeaconColor on the telemetry
     *
     * @param receiver the ResultReceiver that contains the BeaconColor
     */
    public static void displayBeaconColor(ResultReceiver<BeaconColorResult.BeaconColor> receiver) {
        if (receiver.isReady()) {
            telemetry.addData("color", receiver.getValue());
        } else {
            telemetry.addData("receiver not ready", "");
        }
    }

    /**
     * Display whether or not a gyro sensor is calibrated on the telemetry screen
     *
     * @param gyro the gyro sensor
     */
    public static void displayGyroIsCalibrated(GyroSensor gyro) {
        if (gyro.isCalibrating()) {
            telemetry.addData("Gyro state", "CALIBRATING");
        } else {
            telemetry.addData("Gyro state", "ready");
        }
    }
}

StepTimer keeps track of how long it takes for something to execute. For example:

StepTimer stepTimer = new StepTimer("MATH");
stepTimer.start();
double result = doComplexMathCalculation(1, 2, 3); //replace this with your actual code.
stepTimer.log("Complex math calculation");

The output in the Android monitor would be:

01-02 03:04:05.678      901-234/? I/MATH: TIME: Complex math calculation: 567 ms

Here is an example from the image processing code:

stepTimer.start();
//save the raw camera image for logging
ImageUtil.saveImage(TAG, rgbaFrame, Imgproc.COLOR_RGBA2BGR, "00_camera", startTime);
stepTimer.log("save 00");

ftc/evlib/util/StepTimer.java

package ftc.evlib.util;

import android.util.Log;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 8/26/16
 *
 * Records and logs the time it takes to complete a certain step in an algorithm
 */
public class StepTimer {
    private final String tag;
    private long timer;

    /**
     * @param tag the tag to give to the android logging
     *            You can search for the tag in the logs to find messages generated by your code
     */
    public StepTimer(String tag) {
        this.tag = tag;
        start();
    }

    /**
     * Start a step
     */
    public void start() {
        timer = System.nanoTime();
    }

    /**
     * Log the name of the step and the time since it started
     *
     * @param message the name of the step
     */
    public void log(String message) {
        Log.i(tag, "TIME: " + message + ": " + String.valueOf(get()) + " ms");
    }

    /**
     * @return the time since the step started
     */
    public double get() {
        return (System.nanoTime() - timer) / 1000000.0;
    }
}

FileUtil manages a directory structure on the robot controller phone:

• root/sdcard/FTC
  • pictures - outputs of [[ImageProcessor]] operations
  • logs - outputs of the logging from any opmode
  • options - storage for autonomous options
  • configs - storage for servo preset values
    • inside configs is a directory for each [[Robot Configuration]] with its servos' presets

FileUtil can return a reference to any of these directories, or a new File in any of these directories.

ftc/evlib/util/FileUtil.java

package ftc.evlib.util;

import android.os.Environment;

import java.io.File;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/2/16
 *
 * Utilities to get files and directories on the phone storage
 * All files will be under the "FTC" directory in the phone's root directory
 */

public class FileUtil {
    private static final String APP_DIR_NAME = "FTC";

    /**
     * @return a reference to the root phone directory
     */
    public static File getRootDir() {
        return Environment.getExternalStorageDirectory();
    }

    /**
     * @return a reference to the root phone directory
     */
    public static File getAppDir() {
        File dir = new File(getRootDir(), "/" + APP_DIR_NAME);
        mkdirsOrThrowException(dir);
        return dir;
    }

    /**
     * Get a reference to a file in the app directory
     *
     * @param filename the name of the file
     * @return the File
     */
    public static File getAppFile(String filename) {
        return new File(getAppDir(), filename);
    }

    public static File getDir(String dirname) {
        File dir = new File(getAppDir(), dirname);
        mkdirsOrThrowException(dir);
        return dir;
    }

    public static File getFile(String dirname, String filename) {
        return new File(getDir(dirname), filename);
    }

    public static File getPicturesDir() {
//        return Environment.getExternalStoragePublicDirectory(Environment.DIRECTORY_PICTURES);
        return getDir("pictures");
    }

    public static File getPicturesFile(String filename) {
        return new File(getPicturesDir(), filename);
    }

    public static File getLogsDir() {
        return getDir("logs");
    }

    public static File getLogsFile(String filename) {
        return new File(getLogsDir(), filename);
    }

    public static File getOptionsDir() {
        return getDir("options");
    }

    public static File getOptionsFile(String filename) {
        return new File(getOptionsDir(), filename);
    }

    public static File getConfigsDir() {
        return getDir("configs");
    }

    public static File getConfigsFile(String filename) {
        return new File(getConfigsDir(), filename);
    }

    public static void mkdirsOrThrowException(File dir) {
        //noinspection ResultOfMethodCallIgnored
        dir.mkdirs();
        if (!dir.exists() || !dir.isDirectory()) {
            throw new IllegalArgumentException("Error creating directory \"" + dir + '"');
        }
    }
}

Logger logs values in a text file with columns separated by tabs. This file can later be downloaded to your computer and read by a spreadsheet program to graph the values.

When you create it, you pass in:

• The file's name (e.g. "log")
• The file's extension (e.g. ".csv")
• A list of Logger.Column objects, which contain
  • A column title to be put at the top of the file
  • An InputExtractor of any type

Another column is automatically added at the beginning that logs the time in milliseconds

When the start(File dir) method is called, a file is created named <filename><timestamp><file extension> in the specified directory, for example log1234567890.txt. The timestamp is in milliseconds (System.currentTimeMillis()). Then the first line containing the column headers is written.

When the act() method is called, the InputExtractor for each Column is read, the results are connected with tabs, and the joined text is written to a new line in the file.

When the stop() method is called, the file is closed.

The getFileName() method returns the name of the log file.

ftc/electronvolts/util/files/Logger.java

package ftc.electronvolts.util.files;

import java.io.File;
import java.io.FileOutputStream;
import java.io.IOException;
import java.io.PrintStream;
import java.math.RoundingMode;
import java.text.DecimalFormat;
import java.util.List;

import ftc.electronvolts.util.InputExtractor;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * A logger that takes a list of Columns, which have a header and an
 * InputExtractor, and logs each of them to a column in a file
 */
public class Logger {
    public static class Column {
        private final String header;
        private final InputExtractor<?> input;

        public Column(String header, InputExtractor<?> input) {
            this.header = header;
            this.input = input;
        }
    }

    private static final DecimalFormat df = new DecimalFormat("#.#####");
    static {
        df.setRoundingMode(RoundingMode.HALF_UP);
    }

    private long logStart;
    private PrintStream fileStream;
    private final String fileName, fileExtension;
    private String titles;
    private String fullFileName;
    private final List<Column> columns;

    /**
     * @param fileName the name of the file
     * @param fileExtension the file's extension (.txt for example)
     * @param columns the columns that will be written to the file
     */
    public Logger(String fileName, String fileExtension, List<Column> columns) {
        this.fileName = fileName;
        this.fileExtension = fileExtension;
        StringBuilder sb = new StringBuilder("time");
        for (Column column : columns) {
            sb.append("\t").append(column.header);
        }
        titles = sb.append("\n").toString();
        this.columns = columns;
    }

    /**
     * write the column titles to the file
     */
    public boolean start(File dir) {
        logStart = System.nanoTime();

        fullFileName = fileName + System.currentTimeMillis() + fileExtension;
        File file = new File(dir, fullFileName);
        try {
            fileStream = new PrintStream(new FileOutputStream(file));

            fileStream.printf(titles);
            return true;
        } catch (IOException e) {
            return false;
        }
    }

    /**
     * write the input columns to the file
     */
    public void act() {
        if (fileStream != null) {
            long now = System.nanoTime();
            StringBuilder line = new StringBuilder(df.format(1e-6 * (now - logStart)));
            for (Column column : columns) {
                line.append("\t").append(column.input.getValue());
            }
            fileStream.printf(line.append("\n").toString());
        }
    }

    /**
     * close the file
     */
    public void stop() {
        if (fileStream != null) {
            fileStream.close();
        }
    }

    public String getFileName() {
        return fullFileName;
    }
}

OptionsFile is used to store values in a file and manage the conversions of those values to certain types. You can create an OptionsFile in three ways. In all three, a Converters object is required.

OptionsFile opts = new OptionsFile(UtilConverters.getInstance(), File file); //from an existing file
OptionsFile opts = new OptionsFile(UtilConverters.getInstance(), Map<String, String> map); //from a map of String to String
OptionsFile opts = new OptionsFile(UtilConverters.getInstance()); //from thin air!

Then you can add items to it:

String name = "Mr. Prof. Dr. Adm. Rev. Sir Gerald Archibald Edgar Shelby Bartholomew Reginald Eggbert Mortimer Alphonso Smith XLII, Jr.";
int age = 83; //shoutout to my octogenarians
double height = 98.8; //inches
TeamColor teamColor = TeamColor.BLUE;

opts.set("name", name);
opts.set("age", age);
opts.set("height", height);
opts.set("teamColor", teamColor);

Or get items from it:

String name = opts.get("name", String.class); //get the name
Integer age = opts.get("age", 0); //get the age. If not found, use 0.
Double height = opts.get("height", 0.0); //get the height. If not found, use 0.0.
TeamColor teamColor = opts.get("teamColor", null); //get the teamColor. If not found, use null.

Or save it to a file:

if (opts.writeToFile(new File("person.txt"))) {
    System.out.println("Save success!");
} else {
    System.out.println("File not found!");
}

You can also set and get arrays of values of any type supported by the given Converters.

Integer[] array1 = { 1, 2, 3 };
opts.setArray("array", array1);
Integer[] array2 = opts.getArray("array", Integer.class);

TeamColor[] array3 = { TeamColor.RED, TeamColor.RED, TeamColor.BLUE };
opts.setArray("array", array3);
TeamColor[] array4 = opts.getArray("array", TeamColor.class);

ftc/electronvolts/util/OptionsFile.java

package ftc.electronvolts.util.files;

import java.io.BufferedReader;
import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileReader;
import java.io.FileWriter;
import java.io.IOException;
import java.lang.reflect.Array;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.IllegalFormatConversionException;
import java.util.List;
import java.util.Map;
import java.util.MissingResourceException;
import java.util.regex.Pattern;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * This class stores and retrieves values from a file. It should probably be
 * replaced by an XML or JSON interpreter.
 */
public class OptionsFile {
    private static final String DEFAULT_ARRAY_SEPARATOR = ",";
    private static final String DEFAULT_SEPARATOR = "=";

    /**
     * converts objects to and from strings
     */
    private final Converters converters;

    /**
     * stores the strings that are read to and written from files
     */
    private Map<String, String> values;

    /**
     * @param converters the utilities that convert strings to and from objects
     */
    public OptionsFile(Converters converters) {
        this.converters = converters;
        values = new HashMap<>();
    }

    /**
     * @param converters the utilities that convert strings to and from objects
     * @param values the map of values to be loaded
     */
    public OptionsFile(Converters converters, Map<String, String> values) {
        this.converters = converters;
        this.values = values;
    }

    /**
     * retrieve an OptionsFile from a file
     *
     * @param converters the utilities that convert strings to and from objects
     * @param file the file to read from
     * @return the OptionsFile
     */
    public OptionsFile(Converters converters, File file) {
        this.converters = converters;
        values = new HashMap<>();
        BufferedReader br = null;
        try {
            //read each line of the file
            br = new BufferedReader(new FileReader(file));
            String separator = br.readLine();
            if (separator == null) return;
            String line;
            while ((line = br.readLine()) != null) {
                try {
                    //split the line at the "="
                    String[] elements = line.split(separator);

                    //extract the key and value from the split line
                    String key = elements[0].trim();
                    String value = elements[1].trim();

                    //put the key and value into the map
                    values.put(key, value);
                } catch (IndexOutOfBoundsException e) {
                    e.printStackTrace();
                }
            }
        } catch (FileNotFoundException e1) {
            e1.printStackTrace();
        } catch (IOException e1) {
            e1.printStackTrace();
        } finally {
            try {
                if (br != null) {
                    br.close();
                }
            } catch (IOException e) {
                e.printStackTrace();
            }
        }
    }

    /**
     * store the values to a file
     *
     * @param file the file
     * @return whether or not it worked
     */
    public boolean writeToFile(File file) {
        try {
            FileWriter fw = new FileWriter(file);

            String separator = DEFAULT_SEPARATOR;
            boolean done = false;
            while (!done) {
                done = true;
                for (Map.Entry<String, String> entry : values.entrySet()) {
                    if (entry.getKey().contains(separator) || entry.getValue().contains(separator)) {
                        done = false;
                        separator += DEFAULT_SEPARATOR;
                        break;
                    }
                }
            }

            fw.write(separator + "\n");
            for (Map.Entry<String, String> entry : values.entrySet()) {
                fw.write(entry.getKey() + separator + entry.getValue() + "\n");
            }
            fw.close();
            return true;
        } catch (IOException e) {
            e.printStackTrace();
            return false;
        }
    }

    /**
     * @return A map of all the values from the file
     */
    public Map<String, String> getValues() {
        return values;
    }

    /**
     * set a value in the map
     *
     * @param tag the name of the value
     * @param value a boolean
     */
    public void set(String tag, boolean value) {
        //convert the value to a string and add the key-value pair to the values map
        values.put(tag, Boolean.toString(value));
    }

    /**
     * set a value in the map
     *
     * @param tag the name of the value
     * @param value a byte
     */
    public void set(String tag, byte value) {
        //convert the value to a string and add the key-value pair to the values map
        values.put(tag, Byte.toString(value));
    }

    /**
     * set a value in the map
     *
     * @param tag the name of the value
     * @param value a char
     */
    public void set(String tag, char value) {
        //convert the value to a string and add the key-value pair to the values map
        values.put(tag, Character.toString(value));
    }

    /**
     * set a value in the map
     *
     * @param tag the name of the value
     * @param value a short
     */
    public void set(String tag, short value) {
        //convert the value to a string and add the key-value pair to the values map
        values.put(tag, Short.toString(value));
    }

    /**
     * set a value in the map
     *
     * @param tag the name of the value
     * @param value an int
     */
    public void set(String tag, int value) {
        //convert the value to a string and add the key-value pair to the values map
        values.put(tag, Integer.toString(value));
    }

    /**
     * set a value in the map
     *
     * @param tag the name of the value
     * @param value a long
     */
    public void set(String tag, long value) {
        //convert the value to a string and add the key-value pair to the values map
        values.put(tag, Long.toString(value));
    }

    /**
     * set a value in the map
     *
     * @param tag the name of the value
     * @param value a float
     */
    public void set(String tag, float value) {
        //convert the value to a string and add the key-value pair to the values map
        values.put(tag, Float.toString(value));
    }

    /**
     * set a value in the map
     *
     * @param tag the name of the value
     * @param value a double
     */
    public void set(String tag, double value) {
        //convert the value to a string and add the key-value pair to the values map
        values.put(tag, Double.toString(value));
    }

    /**
     * set a value in the map
     *
     * @param tag the name of the value
     * @param object the Object to put into the map
     */
    public <T> void set(String tag, T object) {

        //if the object is null, add a null value to the map
        if (object == null) {
            values.put(tag, null);
            return;
        }

        //get the class to convert to
        Class<T> clazz = (Class<T>) object.getClass();

        //get the converter for the specified class
        Converter<T> converter = converters.getConverter(clazz);

        //throw an error if there is no converter for the class
        if (converter == null) {
            throw new MissingResourceException("No converter given.", converters.getClass().getName(), clazz.getName());
        }

        //convert the value to a string
        String string = converter.toString(object);

        //if the result is null, throw an exception
        if (string == null) throw new IllegalFormatConversionException((char) 0, clazz);

        //add the key-value pair to the values map
        values.put(tag, string);
    }

    /**
     * set an array of values in a map
     * 
     * @param tag the name of the value
     * @param objects the array of objects to put in the map
     */
    public <T> void setArray(String tag, T[] objects) {
        setArray(tag, objects, DEFAULT_ARRAY_SEPARATOR);
    }

    /**
     * set an array of values in a map
     * 
     * @param tag the name of the value
     * @param objects the array of objects to put in the map
     * @param separator the string to join the array elements with. Cannot be in
     *            the output of the conversion for any item of the array
     */
    public <T> void setArray(String tag, T[] objects, String separator) {
        //if the object is null, add a null value to the map
        if (objects == null) {
            values.put(tag, null);
            return;
        }

        //get the class to convert to
        Class<T> clazz = (Class<T>) objects.getClass().getComponentType();

        //get the converter for the specified class
        Converter<T> converter = converters.getConverter(clazz);

        //throw an error if there is no converter for the class
        if (converter == null) {
            throw new MissingResourceException("No converter given for \"" + clazz.getName() + "\".", converters.getClass().getName(), clazz.getName());
        }

        StringBuilder stringBuilder = new StringBuilder();

        boolean first = true;
        for (T object : objects) {
            if (!first) stringBuilder.append(separator);
            first = false;

            //convert the value to a string
            String string = converter.toString(object);

            //if the result is null, throw an exception
            if (string == null) throw new IllegalFormatConversionException((char) 0, clazz);
            if (string.contains(separator)) {
                throw new IllegalArgumentException("Converted input \"" + string + "\" cannot contain separator \"" + separator + "\".");
            }

            //append it to the string builder
            stringBuilder.append(string);
        }

        //build the string
        String string = stringBuilder.toString();

        //add the key-value pair to the values map
        values.put(tag, string);
    }

    /**
     * 
     * @param tag the name of the value
     * @param clazz the class to convert to
     * @return an array of the specified type
     * @throws IllegalArgumentException if there is no converter for the given
     *             type
     */
    public <T> T[] getArray(String tag, Class<T> clazz) {
        return getArray(tag, clazz, DEFAULT_ARRAY_SEPARATOR);
    }

    /**
     * 
     * @param tag the name of the value
     * @param clazz the class to convert to
     * @param separator the string to separate the array elements with (not a
     *            regex)
     * @return an array of the specified type
     * @throws IllegalArgumentException if there is no converter for the given
     *             type
     */
    public <T> T[] getArray(String tag, Class<T> clazz, String separator) {
        //get the converter for the specified class
        Converter<T> converter = converters.getConverter(clazz);

        //throw an error if there is no converter for the class
        if (converter == null) {
            throw new MissingResourceException("No converter given.", converters.getClass().getName(), clazz.getName());
        }

        if (!values.containsKey(tag)) {
            throw new IllegalArgumentException();
        }

        //get the value from the map
        String string = values.get(tag);

        //if the input is null, return null
        if (string == null) return null;

        //separate the string into parts. use the separator as a literal string, not a regex
        String[] parts = string.split(Pattern.quote(separator));

        T[] results = (T[]) Array.newInstance(clazz, parts.length);
        for (int i=0; i<parts.length; i++) {
            String part = parts[i];

            //convert the string to the object
            T result = converter.fromString(part);

            //if the result is null, throw an exception
            if (result == null) throw new IllegalFormatConversionException((char) 0, clazz);

            results[i] = result;
        }

        return results;
    }

    /**
     * @param tag the name of the value
     * @param clazz the class to convert to
     * @return the value converted to the specified type
     * @throws MissingResourceException if there is no converter for the given
     *             type
     * @throws IllegalArgumentException if there is no value with the given tag
     * @throws IllegalFormatConversionException if the string could not be
     *             converted to the specified object
     */
    public <T> T get(String tag, Class<T> clazz) {
        if (clazz == null) {
            throw new IllegalArgumentException("clazz cannot be null.");
        }

        //get the converter for the specified class
        Converter<T> converter = converters.getConverter(clazz);

        //throw an error if there is no converter for the class
        if (converter == null) {
            throw new MissingResourceException("No converter given.", converters.getClass().getName(), clazz.getName());
        }

        if (!values.containsKey(tag)) {
            throw new IllegalArgumentException();
        }

        //get the value from the map
        String string = values.get(tag);

        //if the input is null, return null
        if (string == null) return null;

        //convert the string to the object
        T result = converter.fromString(string);

        //if the result is null, throw an exception
        if (result == null) throw new IllegalFormatConversionException((char) 0, clazz);

        return result;
    }

    /**
     * @param tag the name of the value
     * @param clazz the class to convert to
     * @param fallback the value to use if the conversion fails
     * @return the value converted to the specified type
     */
    public <T> T get(String tag, Class<T> clazz, T fallback) {
        //try to convert, otherwise return the fallback
        try {
            return get(tag, clazz);
        } catch (IllegalArgumentException e) {
            return fallback;
        }
    }

    /**
     * @param tag the name of the value
     * @param fallback the value to use if none is found
     * @return the value converted to the specified type
     * @throws IllegalArgumentException if there is no converter for the given
     *             type
     */
    public <T> T get(String tag, T fallback) {
        //get the class to convert to
        Class<T> clazz = (Class<T>) fallback.getClass();

        return get(tag, clazz, fallback);
    }

}

InputExtractor is an interface that can be any type. It has one method, getValue(), that returns a value of that type. It is used where an input must be passed in, but that input needs to be generic.

InputExtractor is used to get inputs for a variety of uses:

• Logging
• Digital edge detection
• Analog scaling
• Comparing inputs in EndConditions

ftc/electronvolts/util/InputExtractor.java

package ftc.electronvolts.util;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * This class is used to extract a static method and store it in an object
 */
public interface InputExtractor<Type> {
    /**
     * @return the value from wherever the InputExtractor got it
     */
    Type getValue();
}

The InputExtractors factory class has methods for adding, subtracting, multiplying, dividing, etc. two InputExtractor objects or one InputExtractor object and a constant. It also has methods for boolean operators such as and, or, xor, etc.

ftc/electronvolts/util/InputExtractors.java

package ftc.electronvolts.util;

/**
 * This file was made by the electronVolts, FTC team 7393
 * 
 * This is the factory class for InputExtractor
 */
public class InputExtractors {
    private static final InputExtractor<Double> ZERO = constant(0.0);

    /**
     * @return an InputExtractor<Double> that always returns 0
     */
    public static InputExtractor<Double> zero() {
        return ZERO;
    }

    /**
     * @param value the value to return
     * @return an InputExtractor that always returns the specified value
     */
    public static <T> InputExtractor<T> constant(final T value) {
        return new InputExtractor<T>() {
            @Override
            public T getValue() {
                return value;
            }
        };
    }

    /**
     * Multiply an InputExtractor<Double> by a constant
     * 
     * @param inputExtractor the InputExtractor<Double> to multiply
     * @param value the constant to multiply by
     * @return the created InputExtractor<Double>
     */
    public static InputExtractor<Double> multiply(final InputExtractor<Double> inputExtractor, final double value) {
        return new InputExtractor<Double>() {
            @Override
            public Double getValue() {
                return inputExtractor.getValue() * value;
            }
        };
    }

    /**
     * Multiply an InputExtractor<Double> by a constant
     * 
     * @param value the constant to multiply by
     * @param inputExtractor the InputExtractor<Double> to multiply
     * @return the created InputExtractor<Double>
     */
    public static InputExtractor<Double> multiply(final double value, final InputExtractor<Double> inputExtractor) {
        return multiply(value, inputExtractor);
    }

    /**
     * Multiply an InputExtractor<Double> by another InputExtractor<Double>
     * 
     * @param inputExtractor1 the first InputExtractor<Double>
     * @param inputExtractor2 the second InputExtractor<Double>
     * @return the created InputExtractor<Double>
     */
    public static InputExtractor<Double> multiply(final InputExtractor<Double> inputExtractor1, final InputExtractor<Double> inputExtractor2) {
        return new InputExtractor<Double>() {
            @Override
            public Double getValue() {
                return inputExtractor1.getValue() * inputExtractor2.getValue();
            }
        };
    }

    /**
     * Divide an InputExtractor<Double> by a constant. Equivalent to multiplying
     * by the reciprocal of the constant
     * 
     * @param inputExtractor the InputExtractor<Double>
     * @param value the constant
     * @return the created InputExtractor<Double>
     */
    public static InputExtractor<Double> divide(final InputExtractor<Double> inputExtractor, final double value) {
        return multiply(inputExtractor, 1 / value);
    }

    /**
     * Divide an InputExtractor<Double> by another InputExtractor<Double>
     * 
     * @param inputExtractor1 the numerator
     * @param inputExtractor2 the denominator
     * @return
     */
    public static InputExtractor<Double> divide(final InputExtractor<Double> inputExtractor1, final InputExtractor<Double> inputExtractor2) {
        return new InputExtractor<Double>() {
            @Override
            public Double getValue() {
                return inputExtractor1.getValue() / inputExtractor2.getValue();
            }
        };
    }

    /**
     * Divide a constant by an InputExtractor<Double>
     * 
     * @param value the constant
     * @param inputExtractor the InputExtractor<Double>
     * @return the created InputExtractor<Double>
     */
    public static InputExtractor<Double> divide(final double value, final InputExtractor<Double> inputExtractor) {
        return new InputExtractor<Double>() {
            @Override
            public Double getValue() {
                return value / inputExtractor.getValue();
            }
        };
    }

    /**
     * Add a constant to an InputExtractor<Double>
     * 
     * @param inputExtractor the InputExtractor<Double>
     * @param value the constant
     * @return the created InputExtractor<Double>
     */
    public static InputExtractor<Double> add(final InputExtractor<Double> inputExtractor, final double value) {
        return new InputExtractor<Double>() {
            @Override
            public Double getValue() {
                return inputExtractor.getValue() + value;
            }
        };
    }

    /**
     * Add a constant to an InputExtractor<Double>
     * 
     * @param value the constant
     * @param inputExtractor the InputExtractor<Double>
     * @return the created InputExtractor<Double>
     */
    public static InputExtractor<Double> add(final double value, final InputExtractor<Double> inputExtractor) {
        return add(inputExtractor, value);
    }

    /**
     * Add two InputExtractor<Double> objects together
     * 
     * @param inputExtractor1 the first InputExtractor<Double>
     * @param inputExtractor2 the second InputExtractor<Double>
     * @return the created InputExtractor<Double>
     */
    public static InputExtractor<Double> add(final InputExtractor<Double> inputExtractor1, final InputExtractor<Double> inputExtractor2) {
        return new InputExtractor<Double>() {
            @Override
            public Double getValue() {
                return inputExtractor1.getValue() + inputExtractor2.getValue();
            }
        };
    }

    /**
     * Subtract a constant from an InputExtractor<Double>
     * 
     * @param inputExtractor the InputExtractor<Double>
     * @param value the constant to subtract
     * @return the created InputExtractor<Double>
     */
    public static InputExtractor<Double> subtract(final InputExtractor<Double> inputExtractor, final double value) {
        return add(inputExtractor, -value);
    }

    /**
     * Subtract one InputExtractor<Double> from another InputExtractor<Double>
     * 
     * @param inputExtractor1 the first InputExtractor<Double>
     * @param inputExtractor2 the second InputExtractor<Double> (will be
     *            subtracted from the first)
     * @return the created InputExtractor<Double>
     */
    public static InputExtractor<Double> subtract(final InputExtractor<Double> inputExtractor1, final InputExtractor<Double> inputExtractor2) {
        return new InputExtractor<Double>() {
            @Override
            public Double getValue() {
                return inputExtractor1.getValue() - inputExtractor2.getValue();
            }
        };
    }

    /**
     * Subtract an InputExtractor<Double> from a constant
     * 
     * @param value the constant
     * @param inputExtractor the InputExtractor<Double>
     * @return the created InputExtractor<Double>
     */
    public static InputExtractor<Double> subtract(final double value, final InputExtractor<Double> inputExtractor) {
        return new InputExtractor<Double>() {
            @Override
            public Double getValue() {
                return value - inputExtractor.getValue();
            }
        };
    }

    /**
     * Get the absolute value of an InputExtractor<Double>
     * 
     * @param inputExtractor the InputExtractor<Double>
     * @return the created InputExtractor<Double>
     */
    public static InputExtractor<Double> absolute(final InputExtractor<Double> inputExtractor) {
        return new InputExtractor<Double>() {
            @Override
            public Double getValue() {
                return Math.abs(inputExtractor.getValue());
            }
        };
    }

    /**
     * Get the negative of an InputExtractor<Double>
     * 
     * @param inputExtractor the InputExtractor<Double>
     * @return the created InputExtractor<Double>
     */
    public static InputExtractor<Double> negative(final InputExtractor<Double> inputExtractor) {
        return multiply(inputExtractor, -1);
    }

    /**
     * Apply a scaling function to an InputExtractor<Double>
     * 
     * @param inputExtractor the InputExtractor<Double>
     * @param function the Function to scale by
     * @return the created InputExtractor<Double>
     */
    public static InputExtractor<Double> function(final InputExtractor<Double> inputExtractor, final Function function) {
        return new InputExtractor<Double>() {
            @Override
            public Double getValue() {
                return function.f(inputExtractor.getValue());
            }
        };
    }

    /**
     * Get the inverse of an InputExtractor<Boolean>
     * @param inputExtractor the InputExtractor<Boolean>
     * @return the created InputExtractor<Boolean>
     */
    public static InputExtractor<Boolean> not(final InputExtractor<Boolean> inputExtractor) {
        return new InputExtractor<Boolean>() {
            @Override
            public Boolean getValue() {
                return !inputExtractor.getValue();
            }
        };
    }

    /**
     * Apply the "and" operator to 2 InputExtractor<Boolean> objects
     * @param inputExtractor1 the first InputExtractor<Boolean>
     * @param inputExtractor2 the second InputExtractor<Boolean>
     * @return the created InputExtractor<Boolean>
     */
    public static InputExtractor<Boolean> and(final InputExtractor<Boolean> inputExtractor1, final InputExtractor<Boolean> inputExtractor2) {
        return new InputExtractor<Boolean>() {
            @Override
            public Boolean getValue() {
                return inputExtractor1.getValue() && inputExtractor2.getValue();
            }
        };
    }

    /**
     * Apply the "nand" operator to 2 InputExtractor<Boolean> objects
     * @param inputExtractor1 the first InputExtractor<Boolean>
     * @param inputExtractor2 the second InputExtractor<Boolean>
     * @return the created InputExtractor<Boolean>
     */
    public static InputExtractor<Boolean> nand(final InputExtractor<Boolean> inputExtractor1, final InputExtractor<Boolean> inputExtractor2) {
        return new InputExtractor<Boolean>() {
            @Override
            public Boolean getValue() {
                return !(inputExtractor1.getValue() && inputExtractor2.getValue());
            }
        };
    }

    /**
     * Apply the "or" operator to 2 InputExtractor<Boolean> objects
     * @param inputExtractor1 the first InputExtractor<Boolean>
     * @param inputExtractor2 the second InputExtractor<Boolean>
     * @return the created InputExtractor<Boolean>
     */
    public static InputExtractor<Boolean> or(final InputExtractor<Boolean> inputExtractor1, final InputExtractor<Boolean> inputExtractor2) {
        return new InputExtractor<Boolean>() {
            @Override
            public Boolean getValue() {
                return inputExtractor1.getValue() || inputExtractor2.getValue();
            }
        };
    }

    /**
     * Apply the "nor" operator to 2 InputExtractor<Boolean> objects
     * @param inputExtractor1 the first InputExtractor<Boolean>
     * @param inputExtractor2 the second InputExtractor<Boolean>
     * @return the created InputExtractor<Boolean>
     */
    public static InputExtractor<Boolean> nor(final InputExtractor<Boolean> inputExtractor1, final InputExtractor<Boolean> inputExtractor2) {
        return new InputExtractor<Boolean>() {
            @Override
            public Boolean getValue() {
                return !(inputExtractor1.getValue() || inputExtractor2.getValue());
            }
        };
    }

    /**
     * Apply the "xor" operator to 2 InputExtractor<Boolean> objects
     * @param inputExtractor1 the first InputExtractor<Boolean>
     * @param inputExtractor2 the second InputExtractor<Boolean>
     * @return the created InputExtractor<Boolean>
     */
    public static InputExtractor<Boolean> xor(final InputExtractor<Boolean> inputExtractor1, final InputExtractor<Boolean> inputExtractor2) {
        return new InputExtractor<Boolean>() {
            @Override
            public Boolean getValue() {
                return inputExtractor1.getValue() ^ inputExtractor2.getValue();
            }
        };
    }

    /**
     * Apply the "xnor" operator to 2 InputExtractor<Boolean> objects
     * @param inputExtractor1 the first InputExtractor<Boolean>
     * @param inputExtractor2 the second InputExtractor<Boolean>
     * @return the created InputExtractor<Boolean>
     */
    public static InputExtractor<Boolean> xnor(final InputExtractor<Boolean> inputExtractor1, final InputExtractor<Boolean> inputExtractor2) {
        return new InputExtractor<Boolean>() {
            @Override
            public Boolean getValue() {
                return !(inputExtractor1.getValue() ^ inputExtractor2.getValue());
            }
        };
    }
}

DigitalInputEdgeDetector takes an InputExtractor of type Boolean. It detects changes in state between calls of the update() method.

• The justPresssed() method returns true if the input has transitioned from false to true between the last two calls of update().
• The justReleased() method returns true if the input has transitioned from true to false between the last two calls of update().
• The isPressed() method returns the value of the input at the last call of update(), regardless of any transitions.

DigitalInputEdgeDetector also implements InputExtractor<Boolean> and returns the value of the input in the getValue() method.

See also: AnalogInputScaler

ftc/electronvolts/util/DigitalInputEdgeDetector.java

package ftc.electronvolts.util;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * A class that is very useful for joystick control.
 * It does edge detection for a digital input.
 */
public class DigitalInputEdgeDetector implements InputExtractor<Boolean> {
    private Boolean currentValue = null;
    private Boolean previousValue = null;
    private InputExtractor<Boolean> extractor;

    /**
     * @param extractor the InputExtractor to do edge detection on
     */
    public DigitalInputEdgeDetector(InputExtractor<Boolean> extractor) {
        this.extractor = extractor;
    }

    /**
     * update the current and previous value of the input
     *
     * @return the current value of the input
     */
    public boolean update() {
        // if this is the first call to update()
        if (currentValue == null) {
            // set currentValue and previousValue to the reading so no edges are
            // triggered
            currentValue = extractor.getValue();
            previousValue = currentValue;
        } else {
            previousValue = currentValue;
            currentValue = extractor.getValue();
        }
        return currentValue;
    }

    /**
     * @return whether or not the input is true right now
     */
    @Override
    public Boolean getValue() {
        return currentValue;
    }

    /**
     * @return whether or not the input is true right now
     */
    public boolean isPressed() {
        return currentValue;
    }

    /**
     * @return if the input just turned from false to true
     */
    public boolean justPressed() {
        return currentValue && !previousValue;
    }

    /**
     * @return if the input just turned from true to false
     */
    public boolean justReleased() {
        return !currentValue && previousValue;
    }
}

AnalogInputScaler takes an InputExtractor of type Double, and a Function to scale the input with.

• The update() method gets the value from the InputExtractor and feeds it through the Function
• The getValue() method returns the scaled value as it was computed by the last call to update()
• The getRawValue() method returns the raw value as it was retrieved by the last call to update()

AnalogInputScaler also implements InputExtractor<Double>, which means it can be passed in to a Logging function, for example.

See also: DigitalInputEdgeDetector

ftc/electronvolts/util/AnalogInputScaler.java

package ftc.electronvolts.util;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * Manages the scaling of an analog input such as a joystick
 */
public class AnalogInputScaler implements InputExtractor<Double> {
    private double rawValue = 0;
    private double value = 0;

    private final InputExtractor<Double> extractor;
    private final Function inputScaler;

    /**
     * @param extractor   the input extractor
     * @param inputScaler the input scaler to use for scaling the input
     */
    public AnalogInputScaler(InputExtractor<Double> extractor, Function inputScaler) {
        this.extractor = extractor;
        this.inputScaler = inputScaler;
    }

    /**
     * updates the output value and raw value
     *
     * @return the scaled value
     */
    public double update() {
    	rawValue = extractor.getValue();
        value = inputScaler.f(rawValue);
        return value;
    }

    /**
     * @return the value
     */
    @Override
    public Double getValue() {
        return value;
    }

    /**
     * @return the raw value of the input
     */
    public double getRawValue() {
        return rawValue;
    }
}

Functions in the state-machine-framework are defined as interfaces. The interface has one method, double f(double x), that defines the input and output of the function. Functions are used in AnalogInputScaler to scale inputs such as joystick values.

ftc/electronvolts/util/Function.java

package ftc.electronvolts.util;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * An interface that defines a scaling function
 */
public interface Function {
    double f(double x);
}

To create Function implementations easily, use the Functions factory class. It has methods for creating Function objects that are polynomial, logarithmic, exponential, etc. Note: the squared() function retains the sign so it can be used for joystick scaling.

ftc/electronvolts/util/Functions.java

package ftc.electronvolts.util;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * Factory class for InputScaler
 * These can be used for joystick scaling
 */
public class Functions {
    /**
     * A constant function y(x) = c
     * 
     * @param c the constant
     * @return the created Function
     */
    public static Function constant(final double c) {
        return new Function() {
            @Override
            public double f(double x) {
                return c;
            }
        };
    }

    /**
     * A linear function y(x) = mx + b
     * 
     * @param m the slope
     * @param b the y-intercept
     * @return the created Function
     */
    public static Function linear(final double m, final double b) {
        return new Function() {
            @Override
            public double f(double x) {
                return m * x + b;
            }
        };
    }

    /**
     * A quadratic function y(x) = ax^2 + bx + c
     * 
     * @param a the squared coefficient
     * @param b the linear coefficient
     * @param c the constant coefficient
     * @return the created Function
     */
    public static Function quadratic(final double a, final double b, final double c) {
        return new Function() {
            @Override
            public double f(double x) {
                return a * x * x + b * x + c;
            }
        };
    }

    /**
     * A cubic function y(x) = ax^3 + bx^2 + cx + d
     * 
     * @param a the cubed coefficient
     * @param b the squared coefficient
     * @param c the linear coefficient
     * @param d the constant coefficient
     * @return the created Function
     */
    public static Function cubic(final double a, final double b, final double c, final double d) {
        return new Function() {
            @Override
            public double f(double x) {
                return a * x * x * x + b * x * x + c * x + d;
            }
        };
    }

    /**
     * A polynomial function:
     * y(x) = a[n]*x^n + a[n-1]*x^(n-1) + ... + a[2]*x^2 + a[1]*x + a[0]
     * note: the first coefficient is the constant term, not the highest term
     * 
     * @param coefficients the constants for each x term
     * @return the created Function
     */
    public static Function polynomial(final double[] coefficients) {
        return new Function() {
            @Override
            public double f(double x) {
                double output = 0;
                double xPart = 1;
                for (int i = 0; i < coefficients.length; i++) {
                    output += coefficients[i] * xPart;
                    xPart *= x;
                }
                return output;
            }
        };
    }

    /**
     * The output = the input squared with the sign retained
     *
     * @return the created Function
     */
    public static Function squared() {
        return new Function() {
            @Override
            public double f(double x) {
                return x * x * Math.signum(x);
            }
        };
    }

    /**
     * The output = the input cubed
     *
     * @return the created Function
     */
    public static Function cubed() {
        return new Function() {
            @Override
            public double f(double x) {
                return x * x * x;
            }
        };
    }

    /**
     * @param deadZone the deadzone to use
     * @return the created Function
     */
    public static Function deadzone(final DeadZone deadZone) {
        return new Function() {
            @Override
            public double f(double x) {
                if (deadZone.isInside(x)) {
                    return 0;
                } else {
                    return x;
                }
            }
        };
    }

    /**
     * limits the input to between min and max
     *
     * @param min the min value
     * @param max the min value
     * @return the created Function
     */
    public static Function limit(final double min, final double max) {
        return new Function() {
            @Override
            public double f(double x) {
                return Utility.limit(x, min, max);
            }
        };
    }

    /**
     * Combines 2 Functions like a composite function b(a(x))
     *
     * @param inner a(x)
     * @param outer b(x)
     * @return the created Function
     */
    public static Function composite(final Function inner, final Function outer) {
        return new Function() {
            @Override
            public double f(double x) {
                return outer.f(inner.f(x));
            }
        };
    }

    /**
     * No modification to the input
     *
     * @return the created Function
     */
    public static Function none() {
        return new Function() {
            @Override
            public double f(double x) {
                return x;
            }
        };
    }

    /**
     * Multiplies the input by a constant
     *
     * @param m the slope
     * @return the created Function
     */
    public static Function linear(final double m) {
        return linear(m, 0);
    }

    /**
     * Logarithmic scaling
     *
     * @param logBase the base of the logarithm
     * @return the created Function
     */
    public static Function logarithmic(final double logBase) {
        return new Function() {
            @Override
            public double f(double x) {
                if (logBase > 0) {
                    double sign = Math.signum(x);
                    x = Math.abs(x);
                    // a log function including the points (0,0) and (1,1)
                    return sign * Math.log(logBase * x + 1) / Math.log(logBase + 1);
                } else {
                    return x;
                }
            }
        };
    }

    /**
     * e^ax based scaling
     * for a != 0
     * y(x) = signum(x) * (e^abs(ax)-1)/(e^a-1)
     * 
     * for a = 0
     * y(x) = x
     * 
     *
     * @param a constant value that determines how curved the function is
     * @return the created Function
     */
    public static Function eBased(final double a) {
        return new Function() {
            @Override
            public double f(double x) {
                if (a == 0) {
                    return x;
                } else {
                    double sign = Math.signum(x);
                    x = Math.abs(x);
                    // a e-based function including the points (0,0) and (1,1)
                    return sign * Math.expm1(a*x)/Math.expm1(a);
                }
            }
        };
    }

    /**
     * a line from (-1,-1) to (-x,-y) to (0,0) to (x,y) to (1,1)
     *
     * @param pointX x
     * @param pointY y
     * @param maxValue the maximum value of the input
     * @return the created Function
     */
    public static Function piecewise(double pointX, double pointY, double maxValue) {
        final double x = Utility.motorLimit(Math.abs(pointX));
        final double y = Utility.motorLimit(Math.abs(pointY));
        final double max = Utility.motorLimit(Math.abs(maxValue));
        final double slope1;
        if (x == 0) {
            slope1 = 0;
        } else {
            slope1 = y / x;
        }
        final double slope2;
        if (x == 1) {
            slope2 = 0;
        } else {
            slope2 = (y - max) / (x - 1);
        }
        return new Function() {
            public double f(double input) {
                if (Math.abs(input) < x) {
                    return slope1 * input;
                } else {
                    return Utility.motorLimit((slope2 * (Math.abs(input) - x) + y) * Math.signum(input));
                }
            }
        };
    }
}

ResultReceiver is an interface that allows you to pass results between threads.

ftc/electronvolts/util/ResultReceiver.java

package ftc.electronvolts.util;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * Used to transmit information between threads
 */
public interface ResultReceiver<T> {
    /**
     * @return whether or not the value been stored yet
     */
    boolean isReady();

    /**
     * @return the value
     */
    T getValue();

    /**
     * @param value the value to be set
     */
    void setValue(T value);

    /**
     * reset the value and mark as not ready
     */
    void clear();
}

BasicResultReceiver is the simplest implementation of the ResultReceiver interface.

ftc/electronvolts/util/BasicResultReceiver.java

package ftc.electronvolts.util;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * The most basic implementation of ResultReceiver
 */
public class BasicResultReceiver<T> implements ResultReceiver<T> {
    private T value = null;
    private boolean ready = false;

    @Override
    public boolean isReady() {
        return ready;
    }

    @Override
    public T getValue() {
        return value;
    }

    @Override
    public void setValue(T value) {
        this.value = value;
        ready = true;
    }

    @Override
    public void clear() {
        ready = false;
        value = null;
    }
}

TeamColor is an enum that represents the alliance a robot is on. It has 3 possible values: RED, BLUE, and UNKNOWN. To convert a string to a TeamColor, use the TeamColor.fromString() method. This method converts the input to capital letters. Then it searches in the string for "RED" and "BLUE". If it doesn't find either one, it then searches for "R" and "B".

TeamColor also has the opposite() method, which can be used to get your opponent's color.

ftc/electronvolts/util/TeamColor.java

package ftc.electronvolts.util;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * This is a class that stores which team you are on.
 */
public enum TeamColor {
    RED,
    BLUE,
    UNKNOWN;

    /**
     * This method converts a String to a TeamColor. Neither color has
     * precedence over the other,
     * which means that when it finds both, it will return UNKNOWN.
     * 
     * @param s the input String
     * @return the TeamColor corresponding to that string
     */
    public static TeamColor fromString(String s) {
    	if (s==null)return UNKNOWN;
        s = s.toUpperCase(); // convert the input to upper case

        boolean foundRed = s.contains("RED"); // look for RED in the input
        boolean foundBlue = s.contains("BLUE"); // look for BLUE in the input

        if (foundRed && !foundBlue) return RED; // if it found RED but not BLUE
        if (foundBlue && !foundRed) return BLUE; // if it found BLUE but not RED
        if (foundBlue && foundRed) return UNKNOWN; // if it found both

        // only case remaining is that neither were found

        boolean foundR = s.contains("R"); // look for R in the input
        boolean foundB = s.contains("B"); // look for B in the input

        if (foundR && !foundB) return RED; // if it found R but not B
        if (foundB && !foundR) return BLUE; // if it found B but not R
        return UNKNOWN; // if it found both or neither.
    }
    
    /**
     * @return the opposite color
     */
    public TeamColor opposite() {
        switch (this) {
            case RED:
                return BLUE;
            case BLUE:
                return RED;
            default:
                return UNKNOWN;
        }
    }
}

MatchTimer keeps track of the current time relative to the start of a match. When you create it, you pass in the length of the match in milliseconds. Then you call the start() method when the match starts. You have to call the update() method every loop.

There are several useful methods available:

• getDeltaTime() returns the time since the last update()
• getElapsedTime() returns the time since the start()
• getTimeLeft() returns the time until the match is over
• isMatchOver() returns true if the match is over
• isMatchJustOver() returns true the first time the match is over when it is called
• getMatchLengthMillis() returns the length of the match in milliseconds
• getStartTime() returns the time the start() method was called
• getNow() returns the last time update() was called

ftc/electronvolts/util/MatchTimer.java

package ftc.electronvolts.util;

import ftc.electronvolts.util.units.Time;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * A simple class for keeping track of elapsed time
 */
public class MatchTimer {
    private final long matchLengthMillis;
    private long startTime, deltaTime, now;
    private boolean hasStopped = false;

    /**
     * The match-timer needs to know how long the match will last.
     *
     * @param matchLengthMillis the length of the match. If negative, the match
     *            continues indefinitely
     */
    public MatchTimer(long matchLengthMillis) {
        this.matchLengthMillis = matchLengthMillis;
    }

    public MatchTimer(Time matchLength) {
        if (matchLength == null) {
            this.matchLengthMillis = -1;
        } else {
            this.matchLengthMillis = (long) matchLength.milliseconds();
        }
    }

    /**
     * Start the match timer. Should be called in init method of project.
     */
    public void start() {
        startTime = System.currentTimeMillis();
        now = startTime;
        deltaTime = 0;

        hasStopped = false;
    }

    /**
     * Update the match timer. Must be called every loop.
     *
     * @return the time between the last call of update() and now
     */
    public long update() {
        long previousTime = now;
        now = System.currentTimeMillis();
        deltaTime = now - previousTime;
        previousTime = now;
        return deltaTime;
    }

    /**
     * Get the time since the last update.
     *
     * @return time since last update
     */
    public long getDeltaTime() {
        return deltaTime;
    }

    /**
     * Get the total time that the match has been going
     *
     * @return total time that the match has been going
     */
    public long getElapsedTime() {
        return now - startTime;
    }

    /**
     * Get the time left in a match
     *
     * @return the time left in a match
     */
    public long getTimeLeft() {
        return matchLengthMillis - getElapsedTime();
    }

    /**
     * Return if the match is over
     *
     * @return returns true if the match is over
     */
    public boolean isMatchOver() {
        return matchLengthMillis >= 0 && now - startTime >= matchLengthMillis;
    }

    /**
     * @return if this is the first loop that the match has been over
     */
    public boolean isMatchJustOver() {
        if (isMatchOver() && !hasStopped) {
            hasStopped = true;
            return true;
        }
        return false;
    }

    /**
     * @return the length of the match in milliseconds
     */
    public long getMatchLengthMillis() {
        return matchLengthMillis;
    }

    /**
     * @return the time the match started in milliseconds
     */
    public long getStartTime() {
        return startTime;
    }

    /**
     * @return the time of the last call to update()
     */
    public long getNow() {
        return now;
    }
}

StateTimer is a simple timer that can be used in a simple state machine. It is not used in the current state machine framework, but it is here in case anyone needs it for something. For a timer that is used in the framework, see EndConditions.

ftc/electronvolts/util/StateTimer.java

package ftc.electronvolts.util;

/**
 * This file was made by the electronVolts, FTC team 7393
 *
 * A timer that can be used in a simplistic state machine
 */
public class StateTimer {
    private long endTime;

    /**
     * Start the timer
     *
     * @param durationMillis how long the timer will run in milliseconds
     */
    public void init(long durationMillis) {
        this.endTime = durationMillis + System.currentTimeMillis();
    }

    /**
     * @return whether or not the time has elapsed yet
     */
    public boolean isDone() {
        return System.currentTimeMillis() >= endTime;
    }
}

Options

The unfortunate part about programming for the FTC robot is that to test code, you must build, download, then run; for large codebases this is often a slow process. Sometimes, there will be numbers which are not necessarily fixed, and rebuilding just before every competition and match is infeasible.

This collection of classes gives you a dynamic solution to this problem:

• ftc.evlib.opmodes.AbstractOptionsOp
• ftc.evlib.util.FileUtil
• ftc.electronvolts.util.OptionsFile
• ftc.electronvolts.util.OptionEntry

These classes allow you to set variables by running an AbstractOptionsOp, then access them later in an unrelated Opmode.

NOTE: Option setting is an opt-in feature. An opmode for setting options does not show up by default.

Interface

You'll probably want to orient your driver hub to be landscape to use the interface

TODO: Add picture of driver hub

The table on the right shows a list of option names and their current values.

• To select an option, press dpad-up or dpad-down (navigate up or down) until you reach it (the arrows are on either side of the option).
• For discrete variables (e.g. integers), press the left/right bumpers to modify their values (this change will be reflected on the telemetry log).
• For approximate continuous variables (e.g. floating-points), press the left/right triggers.
  • The depth of the trigger press will be reflected in the rate at which the option is modified (the heavier the touch, the more the variable goes up).
• To save the options, press the start button
  • The options will not save the opmode finishes but the start button is not pressed
• To clear changes to the last save, press the reset button
  • This will irrevocably remove all changes that were not saved

Defining Variables

You'll first want to set up two classes: An OpMode extending AbstractOptionsOp and an enum class implementing OptionEntry. Here is an example:

// ExampleOptions.java

import ftc.electronvolts.util.OptionEntry;

public enum ExampleOptions implements OptionEntry {

    TypeData<?> data;

    @Override
    public TypeData<?> getData {
        return data;
    }

    ExampleOptions(TypeDataBuilder<?> t) {
        data = t.build();
    }

}

// ExampleOptionsOp.java

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import ftc.evlib.opmodes.AbstractOptionsOp;

@TeleOp(name = "Example Options Op")
public class ExampleOptionsOp extends AbstractOptionsOp {
    @Override
    protected void setImplementationSpecificDetails() {

        filename = "example_options.json";
        options = ExampleOptions.class;

    }
}

These classes would run fine by themselves, but if you try running "Example Options Op" you would see nothing. To create options, you would have to define an entry in ExampleOptions. Let's define one that is heavily suited for this task: The alliance color. It's one you'll only know on the field, and will not necessarily have the time to set.

Let's say that you represent your alliance color like so:

// AllianceColor.java

public enum AllianceColor {
    RED,
    BLUE,
}

We'll define an option with these properties:

• It is called ALLIANCE_COLOR
• It stores the AllianceColor enum
• We somehow tipped off the judge, so we're expecting it to mostly be red

Here is the full definition:

ALLIANCE_COLOR(TypeDataBuilder
    .enumType(AllianceColor.class)
    .withFallback(AllianceColor.RED)
)

Here it is inserted into ExampleOptions:

public enum ExampleOptions implements OptionEntry {

    ALLIANCE_COLOR(TypeDataBuilder
        .enumType(AllianceColor.class)
        .withFallback(AllianceColor.RED)
    )

    TypeData<?> data;

    @Override
    public TypeData<?> getData {
        return data;
    }

    ExampleOptions(TypeDataBuilder<?> t) {
        data = t.build();
    }

}

Now if you run "Example Options Op" again, it will show this one option, which you can now change using the bumpers on your controller. Adding more options is as easy as adding more enum variants.

TODO: add an image

Fetching Options

After you have set these options, you can programatically access them using the OptionsFile class. Admittedly this is not an easy thing to do, and will be the next thing to improve about the option framework.

To get the object containing the options, use:

OptionsFile file = new OptionsFile(FileUtil.getOptionsFile("example_options.json"));

The string "example_options.json" comes from the ExampleOptionsOp defined before.

To get out the AllianceColor, access the object like so:

AllianceColor color = (AllianceColor) file.load(ExampleOptions.ALLIANCE_COLOR);

The cast is necessary, without it you will have a compile error.

This page is out of date

Converters are the method that OptionsFile uses to convert values to and from strings to store in a file.

Converter defines the interface for actually converting something to and from a string.

ftc/electronvolts/util/files/Converter.java

package ftc.electronvolts.util.files;

/**
 * This file was made by the electronVolts, FTC team 7393
 * 
 * This interface defines a conversion from an object of a certain type to a
 * String and back again
 * 
 * @see Converters
 */
public interface Converter<T> {
    /**
     * Convert an object to a String
     * 
     * @param object the object
     * @return a String representing the object
     */
    String toString(T object);

    /**
     * Convert a String to an object
     * 
     * @param string the String
     * @return the object
     */
    T fromString(String string);
}

Converters defines a group of Converter objects for a variety of types of objects. Implementations of Converters are passed to OptionsFile to allow it to convert different types.

ftc/electronvolts/util/files/Converters.java

package ftc.electronvolts.util.files;

/**
 * This file was made by the electronVolts, FTC team 7393
 * 
 * Implementations of this interface hold a Map of Converter objects of
 * different types
 * 
 * @see Converter
 */
public interface Converters {
    /**
     * Get a Converter for a certain class
     * 
     * @param clazz the class that the converter converts
     * @return the Converter
     */
    <T> Converter<T> getConverter(Class<T> clazz);
}

BasicConverters is the most basic implementation of Converters that has Converter objects for Boolean, Byte, Char, Short, Integer, Long, Float, Double, and String.

ftc/electronvolts/util/files/BasicConverters.java

package ftc.electronvolts.util.files;

import java.util.HashMap;
import java.util.Map;

/**
 * This file was made by the electronVolts, FTC team 7393
 * 
 * A basic implementation of Converters that includes Converter objects
 * for all the class versions of the primitive types, and Strings
 * 
 * @see Converters
 * @see Converter
 */
public class BasicConverters implements Converters {
    protected static final Map<Class<? extends Object>, Converter<?>> converterMap = new HashMap<>();
    static {
        converterMap.put(Boolean.class, new Converter<Boolean>() {

            @Override
            public String toString(Boolean object) {
                return object.toString();
            }

            @Override
            public Boolean fromString(String string) {
                return Boolean.valueOf(string);
            }
        });
        converterMap.put(Byte.class, new Converter<Byte>() {

            @Override
            public String toString(Byte object) {
                return object.toString();
            }

            @Override
            public Byte fromString(String string) {
                return Byte.valueOf(string);
            }
        });
        converterMap.put(Character.class, new Converter<Character>() {

            @Override
            public String toString(Character object) {
                return object.toString();
            }

            @Override
            public Character fromString(String string) {
                return string.charAt(0);
            }
        });
        converterMap.put(Short.class, new Converter<Short>() {

            @Override
            public String toString(Short object) {
                return object.toString();
            }

            @Override
            public Short fromString(String string) {
                return Short.valueOf(string);
            }
        });
        converterMap.put(Integer.class, new Converter<Integer>() {

            @Override
            public String toString(Integer object) {
                return object.toString();
            }

            @Override
            public Integer fromString(String string) {
                return Integer.valueOf(string);
            }
        });
        converterMap.put(Long.class, new Converter<Long>() {

            @Override
            public String toString(Long object) {
                return object.toString();
            }

            @Override
            public Long fromString(String string) {
                return Long.valueOf(string);
            }
        });
        converterMap.put(Float.class, new Converter<Float>() {

            @Override
            public String toString(Float object) {
                return object.toString();
            }

            @Override
            public Float fromString(String string) {
                return Float.valueOf(string);
            }
        });
        converterMap.put(Double.class, new Converter<Double>() {

            @Override
            public String toString(Double object) {
                return object.toString();
            }

            @Override
            public Double fromString(String string) {
                return Double.valueOf(string);
            }
        });

        converterMap.put(String.class, new Converter<String>() {

            @Override
            public String toString(String object) {
                return object;
            }

            @Override
            public String fromString(String string) {
                return string;
            }
        });

    }

    private static Converters INSTANCE = new BasicConverters();

    protected BasicConverters() {
    }

    public static Converters getInstance() {
        return INSTANCE;
    }

    @Override
    public <T> Converter<T> getConverter(Class<T> clazz) {
        return (Converter<T>) converterMap.get(clazz);
    }

}

UtilConverters extends BasicConverters, inheriting all its Converter objects. It adds Converters for TeamColor, Vector2D, and Vector3D.

ftc/electronvolts/util/files/UtilConverters.java

package ftc.electronvolts.util.files;

import java.util.regex.Matcher;
import java.util.regex.Pattern;

import ftc.electronvolts.util.TeamColor;
import ftc.electronvolts.util.Vector2D;
import ftc.electronvolts.util.Vector3D;

/**
 * This file was made by the electronVolts, FTC team 7393
 * 
 * An implementation of Converters that extends BasicConverters and adds
 * Converter objects for some of the utility classes
 * 
 * @see BasicConverters
 */
public class UtilConverters extends BasicConverters {
    private static final String DECIMAL_NUMBER = "([0-9\\.\\-]+)";
    private static final String COMMA = " *, *";
    static {
        converterMap.put(TeamColor.class, new Converter<TeamColor>() {

            @Override
            public String toString(TeamColor object) {
                return object.name();
            }

            @Override
            public TeamColor fromString(String string) {
                return TeamColor.fromString(string);
            }
        });
        converterMap.put(Vector2D.class, new Converter<Vector2D>() {

            @Override
            public String toString(Vector2D object) {
                return object.toString();
            }

            @Override
            public Vector2D fromString(String string) {
                Pattern pattern = Pattern.compile("\\(" + DECIMAL_NUMBER + COMMA + DECIMAL_NUMBER + "\\)");
                Matcher matcher = pattern.matcher(string);
                if (matcher.find()) {
                    return new Vector2D(Double.valueOf(matcher.group(1)), Double.valueOf(matcher.group(2)));
                } else {
                    return null;
                }
            }
        });
        converterMap.put(Vector3D.class, new Converter<Vector3D>() {

            @Override
            public String toString(Vector3D object) {
                return object.toString();
            }

            @Override
            public Vector3D fromString(String string) {
                Pattern pattern = Pattern.compile("\\(" + DECIMAL_NUMBER + COMMA + DECIMAL_NUMBER + COMMA + DECIMAL_NUMBER + "\\)");
                Matcher matcher = pattern.matcher(string);
                if (matcher.find()) {
                    return new Vector3D(Double.valueOf(matcher.group(1)), Double.valueOf(matcher.group(2)), Double.valueOf(matcher.group(3)));
                } else {
                    return null;
                }
            }
        });
    }

    private static final Converters INSTANCE = new UtilConverters();

    public static Converters getInstance() {
        return INSTANCE;
    }

    protected UtilConverters() {
        super();
    }
}

Welcome to the EVLib wiki!

EVLib is designed to assist in the development of very powerful FTC autonomous programs. It provides helper mechanisms for a variety of hardware, like servo speed control.

For starters, check the first chapter for instructions on importing into your project.

This tutorial uses our state-machine-framework library. Click here for the state-machine-framework wiki.

Importing the library into the ftc-app is extremely simple!

Unfortunately right now importing Evlib is not very simple, since the library has been migrated and releases aren't up to date. We're working on it though!

1. Download the latest release EVLib-release.aar. You can also click here.

2. Copy the downloaded library to the (your project root)/libs/ directory.

3. Now you need to tell gradle that EVLib is a dependency. In android studio, edit the TeamCode/build.gradle file and add the following lines:

dependencies {
    implementation files('EVlib-release.aar')
}

Attaching the source (Optional, out of date):

1. Download the repository as a zip file and extract it.

2. When a yellow bar appears at the top of the screen:

3. Click on the blue link that says "Choose Sources..." and a window will pop up.

4. Choose the "EVLib-master/EVLib/src/main/java directory" from the file you downloaded.

You are done! You can look at the next chapter to get you started on using the library.

The sample code is in the repository in the sample/ directory. It has a few sub-folders: v0, v1, etc. Each "version" adds more functionality to the code.

The first sample is explained in the next chapter.

If you are just starting out with FTC, using this library to its fullest extend probably seems intimidating. You can, however, use some of the simple functions of the library to save yourself some headaches. The following is an example of a TeleOp program that uses EVLib for 2 things:

1. Edge detection on the joystick buttons. It might not seem like a big deal, but we spent too much time with flags and nested if statements before moving the edge detection to a dedicated class.

2. Management of the motor commands. The EVLib can "wrap" a DcMotor object into either a Motor object (no encoder) or a MotorEnc object. This wrapper class receives your commands and sends them when you call the update() method.

/sample/v0/SampleTeleOp.java

package org.firstinspires.ftc.teamcode.sample.v0;

import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Servo;

import ftc.electronvolts.util.Function;
import ftc.electronvolts.util.Functions;
import ftc.electronvolts.util.Utility;
import ftc.evlib.driverstation.GamepadManager;
import ftc.evlib.hardware.motors.Motor;
import ftc.evlib.hardware.motors.Motors;
import ftc.evlib.hardware.motors.Stoppers;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/18/16
 *
 * Sample TeleOp that uses some of the EVLib functionality
 */

@TeleOp(name = "SampleTeleOp V0")
public class SampleTeleOp extends OpMode {
    /**
     * Adds edge detection to the gamepad buttons
     * and analog scaling to the joysticks
     */
    private GamepadManager driver1, driver2;

    /**
     * Wraps the motors to easily keep track of motor direction
     */
    private Motor leftMotor, rightMotor;

    /**
     * Servo object from the FTC libraries
     */
    private Servo servo;

    /**
     * The servo's current position from 0 to 1
     */
    private double servoPosition;

    /**
     * Takes care of stopping both motors
     */
    private Stoppers stoppers = new Stoppers();

    @Override
    public void init() {
        //get the motors from the hardwareMap and wrap them with the Motor interface
        //         motor factory class method      hardware name, reversed, brake
        leftMotor = Motors.withoutEncoder(hardwareMap, "leftMotor", false, true, stoppers);
        rightMotor = Motors.withoutEncoder(hardwareMap, "rightMotor", true, true, stoppers);

        //get the servo from the hardwareMap
        servo = hardwareMap.servo.get("servo");
    }

    @Override
    public void start() {
        //use a squared function for the joystick scaling
        Function scalingFunction = Functions.squared();

        //create the two drivers' gamepads
        driver1 = new GamepadManager(gamepad1, scalingFunction);
        driver2 = new GamepadManager(gamepad2, scalingFunction);

        //set the servo's position
        servoPosition = 0.5;
    }

    @Override
    public void loop() {
        //set the left and right motor powers based on the scaled joystick values
        leftMotor.setPower(driver1.left_stick_y.getValue());
        rightMotor.setPower(driver1.right_stick_y.getValue());

        //making use of the edge detection from the GamepadManager

        //if the dpad up was just pressed, increase the servo position
        if (driver1.dpad_up.justPressed()) {
            servoPosition += 0.1;
        }

        //if the dpad down was just pressed, decrease the servo position
        if (driver1.dpad_down.justPressed()) {
            servoPosition -= 0.1;
        }

        //limit the servo position to be in the valid range for servos (0 to 1)
        servoPosition = Utility.servoLimit(servoPosition);

        //send the command to the servo
        servo.setPosition(servoPosition);

        //send the motor powers to the motor controller
        leftMotor.update();
        rightMotor.update();
    }

    @Override
    public void stop() {
        //stop both motors
        stoppers.stop();
    }
}

If you want, you can stop here, you can check out a Logging Example for another simple use of EVLib, or you can move on to Robot Configuration if you want to learn more in depth about the library.

The following is an example of how the Logger and FileUtil classes are used together in an OpMode to log values. The logs will appear in the phone storage in the FTC/logs folder.

This example uses guava to initialize lists, which we have left for alternative methods.

package org.firstinspires.ftc.teamcode.yr2016.opmodes;

import com.google.common.collect.ImmutableList;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import ftc.electronvolts.util.InputExtractor;
import ftc.electronvolts.util.files.Logger;
import ftc.evlib.util.FileUtil;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 1/13/17
 */

@TeleOp(name = "LogTestOp")
public class LogTestOp extends OpMode {
    Logger logger = new Logger("log", ".csv", ImmutableList.of(
            new Logger.Column("gamepad1.left_stick_y", new InputExtractor<Float>() {
                @Override
                public Float getValue() {
                    return gamepad1.left_stick_y;
                }
            }),
            new Logger.Column("gamepad1.right_stick_y", new InputExtractor<Float>() {
                @Override
                public Float getValue() {
                    return gamepad1.right_stick_y;
                }
            })
    ));

    @Override
    public void init() {

    }

    @Override
    public void start() {
        logger.start(FileUtil.getLogsDir());
    }

    @Override
    public void loop() {
        logger.act();
    }

    @Override
    public void stop() {
        logger.stop();
    }
}

If you want to learn more about the library, check out the next chapter.

Before you write an OpMode using the EVLib, you have to make a class that will retrieve the devices (such as motors, servos, and sensors) from the hardwareMap.

Here is an example of this configuration class:

/sample/v1/SampleRobotCfg.java

package org.firstinspires.ftc.teamcode.sample.v1;

import com.qualcomm.robotcore.hardware.HardwareMap;

import ftc.electronvolts.util.units.Distance;
import ftc.electronvolts.util.units.Time;
import ftc.electronvolts.util.units.Velocity;
import ftc.evlib.hardware.config.RobotCfg;
import ftc.evlib.hardware.motors.Motors;
import ftc.evlib.hardware.motors.TwoMotors;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/18/16
 *
 * A basic configuration of a robot with 2 motors, and nothing else.
 * Its purpose is to make the same configuration available to multiple opmodes.
 */

public class SampleRobotCfg extends RobotCfg {
    /**
     * the speed of the robot at 100% power
     * you should replace this with a measured value
     */
    private static final Velocity MAX_SPEED = new Velocity(Distance.fromInches(50), Time.fromSeconds(5));

    /**
     * the drive motors of the robot
     */
    private final TwoMotors twoMotors;

    public SampleRobotCfg(HardwareMap hardwareMap) {
        super(hardwareMap);

        // create the twoMotors object
        twoMotors = new TwoMotors(
                //get the left and right motors and wrap it with the EVLib Motor interface
                Motors.withoutEncoder(hardwareMap, "leftMotor", false, true, stoppers),
                Motors.withoutEncoder(hardwareMap, "rightMotor", true, true, stoppers),
                false, //true for speed mode, false for power mode
                MAX_SPEED
        );
    }

    /**
     * gives the opmodes access to the drive motors
     *
     * @return the drive motors
     */
    public TwoMotors getTwoMotors() {
        return twoMotors;
    }

    @Override
    public void act() {

    }

    @Override
    public void stop() {

    }
}

The next step is to bring the robot to life with a Basic TeleOp Program (next chapter)

After you have set up a RobotCfg, you can now write TeleOp and Autonomous OpModes.

The following is a sample TeleOp program that uses the sample RobotCfg.

/sample/v1/SampleTeleOp.java

package org.firstinspires.ftc.teamcode.sample.v1;

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import ftc.electronvolts.util.Function;
import ftc.electronvolts.util.Functions;
import ftc.electronvolts.util.files.Logger;
import ftc.evlib.opmodes.AbstractTeleOp;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/18/16
 *
 * A sample TeleOp program that will allow you to control 2 motors with the left and right joysticks
 */

@TeleOp(name = "SampleTeleOp V1")
public class SampleTeleOp extends AbstractTeleOp<SampleRobotCfg> {
    @Override
    protected Function getJoystickScalingFunction() {
        //use an exponentially based function for the joystick scaling to allow fine control
        return Functions.eBased(5);
//        return Functions.squared();
//        return Functions.cubed();
//        return Functions.none();
    }

    @Override
    protected SampleRobotCfg createRobotCfg() {
        //create and return a SampleRobotCfg for the library to use
        return new SampleRobotCfg(hardwareMap);
    }

    @Override
    protected Logger createLogger() {
        return null;
    }

    @Override
    protected void setup() {

    }

    @Override
    protected void setup_act() {

    }

    @Override
    protected void go() {

    }

    @Override
    protected void act() {
        //set the motor powers to the joystick values
        robotCfg.getTwoMotors().runMotors(
                driver1.left_stick_y.getValue(),
                driver1.right_stick_y.getValue()
        );
    }

    @Override
    protected void end() {

    }
}

The next step is to make a Basic Autonomous Program (next chapter).

Before you can make an autonomous program, add the following field to your RobotCfg:

    //the speed of the robot at 100% power
    private static final Velocity MAX_SPEED = new Velocity(Distance.fromInches(50), Time.fromSeconds(5));

This represents your robot's speed at 100% power. You can calculate it or determine it by measuring the distance your robot drives in a certain amount of time. The Distance and Time classes support a variety of units (you could technically express you robot's speed in nautical miles per week).

The following is an Autonomous program that drives forward for 2 feet (using the speed defined in the robot config):

/sample/v1/SampleAuto.java

package org.firstinspires.ftc.teamcode.sample.v1;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;

import ftc.electronvolts.statemachine.StateMachine;
import ftc.electronvolts.statemachine.StateMachineBuilder;
import ftc.electronvolts.statemachine.StateName;
import ftc.electronvolts.util.files.Logger;
import ftc.electronvolts.util.units.Distance;
import ftc.evlib.opmodes.AbstractAutoOp;
import ftc.evlib.statemachine.EVStates;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/18/16
 *
 * A sample autonomous that drives forward for 2 feet.
 */

@Autonomous(name = "SampleAuto V1")
public class SampleAuto extends AbstractAutoOp<SampleRobotCfg> {

    /**
     * defines all the possible states for the state machine
     * modify this to have whatever states you want
     */
    private enum S implements StateName {
        DRIVE,
        STOP
    }

    @Override
    public StateMachine buildStates() {
        //create a new builder for the states, starting with the DRIVE state
        StateMachineBuilder b = new StateMachineBuilder(S.DRIVE);

        //define the DRIVE state to drive for 2 feet and move to the STOP state
        b.add(EVStates.drive(S.DRIVE, S.STOP, Distance.fromFeet(2), robotCfg.getTwoMotors(), 0.5));

        //define the STOP state to be empty (and never exit) so the state machine will stop
        b.addStop(S.STOP);

        //build and return the StateMachine
        return b.build();
    }

    @Override
    protected SampleRobotCfg createRobotCfg() {
        return new SampleRobotCfg(hardwareMap);
    }

    @Override
    protected Logger createLogger() {
        return null;
    }

    @Override
    protected void setup_act() {

    }

    @Override
    protected void go() {

    }

    @Override
    protected void act() {

    }

    @Override
    protected void end() {

    }
}

You can simplify this autonomous a bit by customizing the StateMachineBuilder (next chapter).

Currently in the autonomous we have the following:

        //define the DRIVE state to drive for 2 feet and move to the STOP state
        b.add(EVStates.drive(S.DRIVE, S.STOP, Distance.fromFeet(2), robotCfg.getTwoMotors(), 0.5));

It would be nice if we didn't have to specify the robot's max speed and the motors to use for each drive state. The solution for that is to make a class that extends StateMachineBuilder. This custom builder will store the parameters that appear commonly in the drive state and add a convenience method with fewer arguments.

The line above could be changed to:

        //more readable and less tedious to type
        b.addDrive(S.DRIVE, S.STOP, Distance.fromFeet(2), 0.5);

Here is the subclass to StateMachineBuilder with the convenience method:

/sample/v2/SampleStateMachineBuilder.java

package org.firstinspires.ftc.teamcode.sample.v2;

import ftc.electronvolts.statemachine.StateMachineBuilder;
import ftc.electronvolts.statemachine.StateName;
import ftc.electronvolts.util.units.Distance;
import ftc.evlib.hardware.motors.TwoMotors;
import ftc.evlib.statemachine.EVStates;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/18/16
 *
 * A sample extension of the StateMachineBuilder.
 * This subclass adds one convenience method for autonomous opmodes.
 */

public class SampleStateMachineBuilder extends StateMachineBuilder {
    /**
     * The drive motors
     */
    private final TwoMotors twoMotors;

    /**
     * Create a SampleStateMachineBuilder, passing it the drive motors
     * The drive motors will be passed to the drive state every time
     *
     * @param firstStateName the state to start with
     * @param twoMotors      the robot's drive motors
     */
    public SampleStateMachineBuilder(StateName firstStateName, TwoMotors twoMotors) {
        super(firstStateName);
        this.twoMotors = twoMotors;
    }

    /**
     * convenience method for adding a drive state
     *
     * @param stateName     the name of the state
     * @param nextStateName the name of the state to go to after the drive is complete
     * @param distance      the distance to drive
     * @param velocity      the velocity to drive at
     */
    public void addDrive(StateName stateName, StateName nextStateName, Distance distance, double velocity) {
        //add the drive state with the motors and speed from sampleRobotCfg
        add(EVStates.drive(stateName, nextStateName, distance, twoMotors, velocity));
    }
}

We then have to change our creation of the StateMachineBuilder to create this new subclass instead, and then we can simplify the drive state.

Here is the autonomous with both changes:

This example uses guava to initialize lists, which we have left for alternative methods.

/sample/v2/SampleAuto.java

package org.firstinspires.ftc.teamcode.sample.v2;

import com.google.common.collect.ImmutableList;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;

import ftc.electronvolts.statemachine.StateMachine;
import ftc.electronvolts.statemachine.StateName;
import ftc.electronvolts.util.InputExtractor;
import ftc.electronvolts.util.files.Logger;
import ftc.electronvolts.util.units.Distance;
import ftc.evlib.opmodes.AbstractAutoOp;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/18/16
 *
 * A sample autonomous that drives forward for 2 feet.
 * This one uses a custom builder
 */

@Autonomous(name = "SampleAuto V2")
public class SampleAuto extends AbstractAutoOp<SampleRobotCfg> {

    /**
     * defines all the possible states for the state machine
     * modify this to have whatever states you want
     */
    private enum S implements StateName {
        DRIVE,
        STOP
    }

    @Override
    public StateMachine buildStates() {
        //create a new builder for the states, starting with the DRIVE state
        //this time we are using the custom builder and passing it the drive motors from the robotCfg
        SampleStateMachineBuilder b = new SampleStateMachineBuilder(S.DRIVE, robotCfg.getTwoMotors());

        //define the DRIVE state to drive for 2 feet and move to the STOP state

        //the old code without the custom builder:
//        b.add(EVStates.drive(S.DRIVE, S.STOP, Distance.fromFeet(2), robotCfg.getTwoMotors(), 0.5));

        //the new code (more readable and less tedious to type):
        b.addDrive(S.DRIVE, S.STOP, Distance.fromFeet(2), 0.5);

        //define the STOP state to be empty (and never exit) so the state machine will stop
        b.addStop(S.STOP);

        //build and return the StateMachine
        return b.build();
    }

    @Override
    protected SampleRobotCfg createRobotCfg() {
        return new SampleRobotCfg(hardwareMap);
    }

    @Override
    protected Logger createLogger() {
        //it will save logs in /FTC/logs/autonomous[....].csv on the robot controller phone
        return new Logger("autonomous", ".csv", ImmutableList.of(
                //each one of these is a column in the log file
                new Logger.Column("state", new InputExtractor<StateName>() {
                    @Override
                    public StateName getValue() {
                        return stateMachine.getCurrentStateName();
                    }
                }),
                new Logger.Column("leftMotor power", new InputExtractor<Double>() {
                    @Override
                    public Double getValue() {
                        return robotCfg.getTwoMotors().getValue(0);
                    }
                }),
                new Logger.Column("rightMotor power", new InputExtractor<Double>() {
                    @Override
                    public Double getValue() {
                        return robotCfg.getTwoMotors().getValue(1);
                    }
                })
        ));
    }

    @Override
    protected void setup_act() {

    }

    @Override
    protected void go() {

    }

    @Override
    protected void act() {

    }

    @Override
    protected void end() {

    }
}

I might not seem like a big improvement, but it makes a huge difference when you have upwards of 20 drive states you are trying to debug. It can also be applied to states that take more parameters, such as a gyro turn which needs TwoMotors, a GyroSensor, and a rotation speed (which could then be the same for all gyro turns).

The next step is to use Servo Presets to add some features to the servos.

The idea of servo presets is to replace "magic numbers" (numbers whose purpose is not obvious) with named servo positions. For example, instead of 7.341, you can write ArmServoPresets.LEFT -- much more readable.

Another feature of the servo presets is that you can modify the preset values without changing the app and re-uploading to the phone.

The way these two features are implemented is that each servo has an enum of presets:

    /**
     * all the possible values for the arm servo
     */
    public enum ArmServoPresets {
        LEFT,
        MIDDLE,
        RIGHT
    }

and each RobotCfg has a list of servos, each of which has a hardware name (such as "armServo") and a list of presets determined from the enum (such as ArmServoPresets.values()):

    /**
     * defines all the servos on the robot
     */
    public enum SampleServoName implements ServoName {
        //enum name("hardware name", preset enum.values()),
        ARM_SERVO("armServo", ArmServoPresets.values()),
        LEG_SERVO("legServo", LegServoPresets.values());

        private final String hardwareName;
        private final Enum[] presets;

        SampleServoName(String hardwareName, Enum[] presets) {
            this.hardwareName = hardwareName;
            this.presets = presets;
        }

        @Override
        public String getHardwareName() {
            return hardwareName;
        }

        @Override
        public Enum[] getPresets() {
            return presets;
        }

        @Override
        public Class<? extends RobotCfg> getRobotCfg() {
            return SampleRobotCfg.class;
        }


    }

To actually enter the preset values, you need to make and run a ServoTuneOp:

/sample/v3/SampleServoTuneOp.java

package org.firstinspires.ftc.teamcode.sample.v3;

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import ftc.evlib.hardware.config.RobotCfg;
import ftc.evlib.opmodes.AbstractServoTuneOp;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/18/16
 *
 * This opmode is very short since the superclass, AbstractServoTuneOp does most of the work. It
 * allows you to change your servo presets without changing the code and re-deploying it to the
 * phone. This means that you can swap out a servo and re-tune it without having to go into the
 * program and fix magic numbers. Note:  It only works if you use presets everywhere instead of
 * hardcoded values.
 *
 * How to use:
 * Select this opmode from the TeleOp menu and run it.
 * Use the dpad up and down to cycle through all the servos
 * Use the dpad left and right to move through the presets for that servo.
 * Press start to save the current preset of the current servo to the current value.
 *
 * The presets are saved in files that are retrieved when you run other opmodes to find the value of each preset.
 */

@TeleOp(name = "SampleServoTuneOp V3")
public class SampleServoTuneOp extends AbstractServoTuneOp {
    @Override
    protected RobotCfg createRobotCfg() {
        //create a new SampleRobotConfig and return it.
        //the superclass will extract the servos and do the rest.
        return new SampleRobotCfg(hardwareMap);
    }
}

The details of how the servos are initialized are in the next chapter.

Here is the SampleRobotConfig with the servo names and presets added:

/sample/v3/SampleRobotCfg.java

package org.firstinspires.ftc.teamcode.sample.v3;

import com.qualcomm.robotcore.hardware.HardwareMap;

import java.util.Map;

import ftc.electronvolts.util.units.Distance;
import ftc.electronvolts.util.units.Time;
import ftc.electronvolts.util.units.Velocity;
import ftc.evlib.hardware.config.RobotCfg;
import ftc.evlib.hardware.motors.Motors;
import ftc.evlib.hardware.motors.TwoMotors;
import ftc.evlib.hardware.servos.ServoCfg;
import ftc.evlib.hardware.servos.ServoName;
import ftc.evlib.hardware.servos.Servos;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/18/16
 *
 * A more complicated configuration of a robot with 2 motors and 2 servos.
 */

public class SampleRobotCfg extends RobotCfg {
    /**
     * the speed of the robot at 100% power
     * you should replace this with a measured value
     */
    private static final Velocity MAX_SPEED = new Velocity(Distance.fromInches(50), Time.fromSeconds(5));

    /**
     * the drive motors of the robot
     */
    private final TwoMotors twoMotors;

    /**
     * stores all the servos of the robot
     */
    private final Servos servos;

    /**
     * all the possible values for the arm servo
     */
    public enum ArmServoPresets {
        LEFT,
        MIDDLE,
        RIGHT
    }

    /**
     * all the possible values for the leg servo
     */
    public enum LegServoPresets {
        DOWN,
        MIDDLE,
        UP
    }

    /**
     * defines all the servos on the robot
     */
    public enum SampleServoName implements ServoName {
        //enum name("hardware name", preset enum.values()),
        ARM_SERVO("armServo", ArmServoPresets.values()),
        LEG_SERVO("legServo", LegServoPresets.values());

        private final String hardwareName;
        private final Enum[] presets;

        SampleServoName(String hardwareName, Enum[] presets) {
            this.hardwareName = hardwareName;
            this.presets = presets;
        }

        @Override
        public String getHardwareName() {
            return hardwareName;
        }

        @Override
        public Enum[] getPresets() {
            return presets;
        }

        @Override
        public Class<? extends RobotCfg> getRobotCfg() {
            return SampleRobotCfg.class;
        }


    }

    /**
     * Create the SampleRobotCfg with the default servo starting positions
     *
     * @param hardwareMap the hardwareMap from the opmode
     */
    public SampleRobotCfg(HardwareMap hardwareMap) {
        this(hardwareMap, ServoCfg.defaultServoStartPresetMap(SampleServoName.values()));
    }

    /**
     * Create the SampleRobotCfg with custom servo starting positions
     *
     * @param hardwareMap         the hardwareMap from the opmode
     * @param servoStartPresetMap the custom servo starting positions
     */
    public SampleRobotCfg(HardwareMap hardwareMap, Map<ServoName, Enum> servoStartPresetMap) {
        super(hardwareMap);

        // create the twoMotors object
        twoMotors = new TwoMotors(
                //get the left and right motors and wrap it with the EVLib Motor interface
                Motors.withoutEncoder(hardwareMap, "leftMotor", false, true, stoppers),
                Motors.withoutEncoder(hardwareMap, "rightMotor", true, true, stoppers),
                false, //true for speed mode, false for power mode
                MAX_SPEED
        );

        servos = new Servos(ServoCfg.createServoMap(hardwareMap, servoStartPresetMap));
    }

    /**
     * gives the opmodes access to the drive motors
     *
     * @return the drive motors
     */
    public TwoMotors getTwoMotors() {
        return twoMotors;
    }

    /**
     * gives the opmodes access to the servos
     *
     * @return the servos
     */
    @Override
    public Servos getServos() {
        return servos;
    }

    @Override
    public void act() {

    }

    @Override
    public void stop() {

    }
}

With the servos now configured, we can move on to Adding Servos to TeleOp

To use the servos in TeleOp, you have to first get the servo from the robotCfg:

armServo = robotCfg.getServo(SampleRobotCfg.SampleServoName.ARM_SERVO);

Then you can move the servo to a preset with:

armServo.goToPreset(SampleRobotCfg.ArmServoPresets.RIGHT);

You can also specify the speed if you want the servo to turn more slowly:

armServo.goToPreset(SampleRobotCfg.ArmServoPresets.RIGHT, 0.2);

The following is the new TeleOp program with the servos added.

This example uses guava to initialize lists, which we have left for alternative methods.

/sample/v3/SampleTeleOp.java

package org.firstinspires.ftc.teamcode.sample.v3;

import com.google.common.collect.ImmutableList;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import ftc.electronvolts.util.Function;
import ftc.electronvolts.util.Functions;
import ftc.electronvolts.util.InputExtractor;
import ftc.electronvolts.util.files.Logger;
import ftc.evlib.hardware.servos.ServoControl;
import ftc.evlib.opmodes.AbstractTeleOp;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/18/16
 *
 * A sample TeleOp program that will allow you to control 2 motors with the left and right joysticks
 * This one also has controls for 2 servos:
 * arm servo:
 * dpad_left - go to LEFT
 * dpad_down - go to MIDDLE
 * dpad_right - go to RIGHT
 *
 * leg servo:
 * a button - go to DOWN
 * b button - go to MIDDLE
 * x button - go to UP
 *
 * I guess you could say that this opmode cost us ...
 * ... an arm and a leg!
 */

@TeleOp(name = "SampleTeleOp V3")
public class SampleTeleOp extends AbstractTeleOp<SampleRobotCfg> {
    private ServoControl armServo, legServo;

    @Override
    protected Function getJoystickScalingFunction() {
        //use an exponentially based function for the joystick scaling to allow fine control
        return Functions.eBased(5);
//        return Functions.squared();
//        return Functions.cubed();
//        return Functions.none();
    }

    @Override
    protected SampleRobotCfg createRobotCfg() {
        //create and return a SampleRobotCfg for the library to use
        return new SampleRobotCfg(hardwareMap);
    }

    @Override
    protected Logger createLogger() {
        //it will save logs in /FTC/logs/teleop[....].csv on the robot controller phone
        return new Logger("teleop", ".csv", ImmutableList.of(
                //each one of these is a column in the log file
                new Logger.Column("driver1.left_stick_y", driver1.left_stick_y),
                new Logger.Column("driver1.right_stick_y", driver1.right_stick_y),
                new Logger.Column("Accelerometer X", new InputExtractor<Double>() {
                    @Override
                    public Double getValue() {
                        return robotCfg.getAccelerometer().getX();
                    }
                }),
                new Logger.Column("ARM_SERVO position", new InputExtractor<Double>() {
                    @Override
                    public Double getValue() {
                        return armServo.getCurrentPosition();
                    }
                }),
                new Logger.Column("LEG_SERVO position", new InputExtractor<Double>() {
                    @Override
                    public Double getValue() {
                        return legServo.getCurrentPosition();
                    }
                })
        ));
    }

    @Override
    protected void setup() {
        //get the arm and leg servos from the SampleRobotCfg
        armServo = robotCfg.getServo(SampleRobotCfg.SampleServoName.ARM_SERVO);
        legServo = robotCfg.getServo(SampleRobotCfg.SampleServoName.LEG_SERVO);
    }

    @Override
    protected void setup_act() {

    }

    @Override
    protected void go() {

    }

    @Override
    protected void act() {
        //set the motor powers to the joystick values
        robotCfg.getTwoMotors().runMotors(
                driver1.left_stick_y.getValue(),
                driver1.right_stick_y.getValue()
        );

        //if the left dpad was just pressed
        if (driver1.dpad_left.justPressed()) {
            //move the servo arm to the LEFT position
            armServo.goToPreset(SampleRobotCfg.ArmServoPresets.LEFT);
        }
        //if the down dpad was just pressed
        if (driver1.dpad_down.justPressed()) {
            //move the servo arm to the MIDDLE position
            armServo.goToPreset(SampleRobotCfg.ArmServoPresets.MIDDLE);
        }
        //if the right dpad was just pressed
        if (driver1.dpad_right.justPressed()) {
            //move the servo arm to the RIGHT position
            armServo.goToPreset(SampleRobotCfg.ArmServoPresets.RIGHT);
        }


        //if the a button was just pressed
        if (driver1.a.justPressed()) {
            //move the leg servo to the DOWN position
            legServo.goToPreset(SampleRobotCfg.LegServoPresets.DOWN);
        }
        //if the b button was just pressed
        if (driver1.b.justPressed()) {
            //move the leg servo to the MIDDLE position
            legServo.goToPreset(SampleRobotCfg.LegServoPresets.MIDDLE);
        }
        //if the x button was just pressed
        if (driver1.x.justPressed()) {
            //move the leg servo to the UP position
            legServo.goToPreset(SampleRobotCfg.LegServoPresets.UP);
        }
    }

    @Override
    protected void end() {

    }
}

The next step is Adding Servos to Autonomous.

Adding servos to the autonomous is a bit more tricky than adding them to TeleOp. First we have to add convenience methods in the SampleStateMachineBuilder

/sample/v3/SampleStateMachineBuilder.java

package org.firstinspires.ftc.teamcode.sample.v3;

import ftc.electronvolts.statemachine.StateMachineBuilder;
import ftc.electronvolts.statemachine.StateName;
import ftc.electronvolts.util.units.Distance;
import ftc.evlib.hardware.servos.ServoControl;
import ftc.evlib.hardware.servos.ServoName;
import ftc.evlib.statemachine.EVStates;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/18/16
 *
 * A sample extension of the StateMachineBuilder.
 * This subclass adds convenience methods for any opmodes that use the SampleRobotConfig.
 */

public class SampleStateMachineBuilder extends StateMachineBuilder {
    /**
     * The SampleRobotCfg to get the drive motors from
     */
    private final SampleRobotCfg sampleRobotCfg;

    /**
     * Create a SampleStateMachineBuilder, passing it the SampleRobotCfg
     * The SampleRobotCfg's drive motors will be passed to the drive state every time
     *
     * @param firstStateName the state to start with
     * @param sampleRobotCfg the robot's configuration
     */
    public SampleStateMachineBuilder(StateName firstStateName, SampleRobotCfg sampleRobotCfg) {
        super(firstStateName);
        this.sampleRobotCfg = sampleRobotCfg;
    }

    /**
     * convenience method for adding a drive state
     *
     * @param stateName the name of the state
     * @param nextStateName the name of the state to go to after the drive is complete
     * @param distance the distance to drive
     * @param velocity the velocity to drive at
     */
    public void addDrive(StateName stateName, StateName nextStateName, Distance distance, double velocity) {
        //add the drive state with the motors and speed from sampleRobotCfg
        add(EVStates.drive(stateName, nextStateName, distance, SampleRobotCfg.MAX_SPEED, sampleRobotCfg.getTwoMotors(), velocity));
    }

    /**
     * add a servo init state that sets all the servos to their starting positions
     *
     * @param stateName the name of the state
     * @param nextStateName the state to go to after the servos are initialized
     */
    public void addServoInit(StateName stateName, StateName nextStateName) {
        add(EVStates.servoInit(stateName, nextStateName, sampleRobotCfg.getServos()));
    }

    /**
     * turn a servo to a preset
     *
     * @param stateName the name of the state
     * @param nextStateName the state to go to after the servo is done turning
     * @param servoName the name of the servo as defined in SampleRobotCfg.ServoName
     * @param servoPreset the servo preset defined in SampleRobotCfg.ArmServoPresets and SampleRobotCfg.LegServoPresets
     * @param speed the speed to run the servo at
     * @param waitForDone wether or not to wait for the servo to turn to move to the next state
     */
    public void addServo(StateName stateName, StateName nextStateName, ServoName servoName, Enum servoPreset, double speed, boolean waitForDone) {
        //get the servo
        ServoControl servoControl = sampleRobotCfg.getServo(servoName);
        //add the state that will run the command
        add(EVStates.servoTurn(stateName, nextStateName, servoControl, servoPreset, speed, waitForDone));
    }

    /**
     * turn a servo to a preset at max speed
     *
     * @param stateName the name of the state
     * @param nextStateName the state to go to after the servo is done turning
     * @param servoName the name of the servo as defined in SampleRobotCfg.ServoName
     * @param servoPreset the servo preset defined in SampleRobotCfg.ArmServoPresets and SampleRobotCfg.LegServoPresets
     * @param waitForDone wether or not to wait for the servo to turn to move to the next state
     */
    public void addServo(StateName stateName, StateName nextStateName, ServoName servoName, Enum servoPreset, boolean waitForDone) {
        //get the servo
        ServoControl servoControl = sampleRobotCfg.getServo(servoName);
        //add the state that will run the command
        add(EVStates.servoTurn(stateName, nextStateName, servoControl, servoPreset, waitForDone));
    }


}

Then comes the easy part -- adding the servo init and turn states to the autonomous:

/sample/v3/SampleAuto.java

package org.firstinspires.ftc.teamcode.sample.v3;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;

import ftc.electronvolts.statemachine.StateMachine;
import ftc.electronvolts.statemachine.StateName;
import ftc.electronvolts.util.units.Distance;
import ftc.evlib.opmodes.AbstractAutoOp;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 10/18/16
 *
 * A sample autonomous that:
 * 1. Initializes the servos
 * 2. Drives forward 2 feet
 * 3. Turns a servo arm
 */

@Autonomous(name="SampleAuto V3")
public class SampleAuto extends AbstractAutoOp<SampleRobotCfg> {

    /**
     * defines all the possible states for the state machine
     * modify this to have whatever states you want
     */
    private enum S implements StateName {
        SERVO_INIT,
        DRIVE,
        SERVO_ARM,
        STOP
    }

    @Override
    public StateMachine buildStates() {
        //create a new builder for the states, starting with the SERVO_INIT state
        //we are using the custom builder and passing it the robotCfg
        SampleStateMachineBuilder b = new SampleStateMachineBuilder(S.SERVO_INIT, robotCfg);

        //add the servo initialization state
        b.addServoInit(S.SERVO_INIT, S.DRIVE);

        //define the DRIVE state to drive for 2 feet and move to the STOP state
        b.addDrive(S.DRIVE, S.SERVO_ARM, Distance.fromFeet(2), 0.5);

        //add the servo turn state
        b.addServo(S.SERVO_ARM, S.STOP, SampleRobotCfg.SampleServoName.ARM_SERVO, SampleRobotCfg.ArmServoPresets.RIGHT, true);

        //define the STOP state to be empty (and never exit) so the state machine will stop
        b.addStop(S.STOP);

        //build and return the StateMachine
        return b.build();
    }

    @Override
    protected SampleRobotCfg createRobotCfg() {
        return new SampleRobotCfg(hardwareMap);
    }

    @Override
    protected void setup_act() {

    }

    @Override
    protected void go() {

    }

    @Override
    protected void act() {

    }

    @Override
    protected void end() {

    }
}

The next step in the tutorial is Vuforia and OpenCV Beacon Color Detection.

Vuforia and OpenCV are image processing libraries. Vuforia is by PTC and is built in to the latest version of the FTC app. It has high-level functions for tracking objects and augmented reality. It is very useful for finding the position of the beacon target images.

OpenCV is an open source library that is more low-level, and because of this it is possible to write the procedures yourself and optimize them for better performance.

Before you can use this example, you have to:

• Set up OpenCV. A guide is available on our website at http://www.gearbox4h.org/opencv/ (no longer exists)
• Create a Vuforia developer account and get a license key from https://developer.vuforia.com/license-manager

This example uses EVLib functionality to:

• Initialize Vuforia
• Find the "Gears" beacon target image (this is controlled by the TeamColor.RED input)
• Crop the beacon out of the frame based on the target image location
• Find the beacon's color

This example uses guava to initialize lists, which we have left for alternative methods.

sample/v3/SampleVuforiaOp.java

package org.firstinspires.ftc.teamcode.sample.v3;

import com.google.common.collect.ImmutableList;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;

import org.firstinspires.ftc.teamcode.R;

import ftc.electronvolts.statemachine.EndConditions;
import ftc.electronvolts.statemachine.StateMachine;
import ftc.electronvolts.statemachine.StateMachineBuilder;
import ftc.electronvolts.statemachine.StateName;
import ftc.electronvolts.util.BasicResultReceiver;
import ftc.electronvolts.util.InputExtractor;
import ftc.electronvolts.util.ResultReceiver;
import ftc.electronvolts.util.TeamColor;
import ftc.electronvolts.util.files.Logger;
import ftc.electronvolts.util.units.Time;
import ftc.evlib.driverstation.Telem;
import ftc.evlib.hardware.config.RobotCfg;
import ftc.evlib.opmodes.AbstractAutoOp;
import ftc.evlib.statemachine.EVStates;
import ftc.evlib.vision.framegrabber.VuforiaFrameFeeder;
import ftc.evlib.vision.processors.BeaconColorResult;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 12/15/16
 */
@Autonomous(name = "SampleVuforiaOp V3")
public class SampleVuforiaOp extends AbstractAutoOp<RobotCfg> {
    //get a key (for free) and paste it below by creating an account at https://developer.vuforia.com/license-manager
    private static final String LICENSE_KEY = "";

    private static final int FRAME_SIZE = 16;
//    private static final int FRAME_SIZE = 8;
//    private static final int FRAME_SIZE = 64;

    private static final int FRAME_WIDTH = 3 * FRAME_SIZE;
    private static final int FRAME_HEIGHT = 4 * FRAME_SIZE;
//    private static final int FRAME_WIDTH = 11 * FRAME_SIZE;
//    private static final int FRAME_HEIGHT = 9 * FRAME_SIZE;

    private enum S implements StateName {
        WAIT_FOR_VUFORIA_INIT,
        FIND_BEACON_COLOR,
        DISPLAY,
        UNKNOWN_BEACON_COLOR,
        BEACON_COLOR_TIMEOUT
    }

    private ResultReceiver<VuforiaFrameFeeder> vuforiaReceiver;
    private ResultReceiver<BeaconColorResult> beaconColorResult;

    @Override
    public StateMachine buildStates() {
        vuforiaReceiver = VuforiaFrameFeeder.initInNewThread(LICENSE_KEY, R.id.cameraMonitorViewId, FRAME_WIDTH, FRAME_HEIGHT);

        StateMachineBuilder b = new StateMachineBuilder(S.WAIT_FOR_VUFORIA_INIT);
        beaconColorResult = new BasicResultReceiver<>();

        b.addEmpty(S.WAIT_FOR_VUFORIA_INIT, b.ts(EndConditions.receiverReady(vuforiaReceiver), S.FIND_BEACON_COLOR));
        b.add(EVStates.findBeaconColorState(S.FIND_BEACON_COLOR, S.DISPLAY, S.UNKNOWN_BEACON_COLOR, S.BEACON_COLOR_TIMEOUT, Time.fromSeconds(10), vuforiaReceiver, beaconColorResult, TeamColor.RED, 4, true));

        b.add(EVStates.displayBeaconColorResult(S.DISPLAY, beaconColorResult));
        b.add(EVStates.displayBeaconColorResult(S.UNKNOWN_BEACON_COLOR, beaconColorResult));
        b.add(EVStates.displayBeaconColorResult(S.BEACON_COLOR_TIMEOUT, beaconColorResult));

        return b.build();
    }

    @Override
    protected RobotCfg createRobotCfg() {
        return new RobotCfg(hardwareMap) {
            @Override
            public void act() {

            }

            @Override
            public void stop() {

            }
        };
    }

    @Override
    protected Logger createLogger() {
        //it will save logs in /FTC/logs/vuforiaOp[....].csv on the robot controller phone
        return new Logger("vuforiaOp", ".csv", ImmutableList.of(
                //each one of these is a column in the log file
                new Logger.Column("state", new InputExtractor<StateName>() {
                    @Override
                    public StateName getValue() {
                        return stateMachine.getCurrentStateName();
                    }
                }),
                new Logger.Column("beaconColor", new InputExtractor<BeaconColorResult>() {
                    @Override
                    public BeaconColorResult getValue() {
                        return beaconColorResult.getValue();
                    }
                })
        ));
    }

    @Override
    protected void setup_act() {
        Telem.displayVuforiaReadiness(vuforiaReceiver);
    }

    @Override
    protected void go() {

    }

    @Override
    protected void act() {
        telemetry.addData("state", stateMachine.getCurrentStateName());
        Telem.displayVuforiaReadiness(vuforiaReceiver);
    }

    @Override
    protected void end() {

    }
}

This concludes the tutorial for now. Check back later for more updates and sample code, or go on to the Features page for an index of other things you can do with EVLib.

Additional Features

EVLib includes the state-machine-framework, so all the features of the state-machine-framework can be used from EVLib.

• the Motor interfaces
• creating motors and continuous servos
• grouping motors
• driving mecanum wheels
• control of mecanum wheels
• servo configuration
• servo speed control
• abstract opmodes
• abstract teleop
• abstract autonomous
• tuning the servo presets
• selecting options for autonomous
• gamepad button edge detection and joystick scaling
• EVStates factory class
• EVEndConditions factory class
• EVStateMachineBuilder
• analog sensors
• digital sensors
• frame grabbing with OpenCV
• image processing with OpenCV
• line sensor
• double line sensor
• color sensor
• line finder
• averaging analog sensors
• global telemetry
• step timer for logging
• file utilities
• [[LineSensorArray]] that uses the i2c SparkFun Line Follower Array
• frame grabbing with Vuforia
• image processing with Vuforia
• [[Joystick Recorder]] for "playback" in autonomous

Planned Features

• [[TaskWeb]] to manage executing and moving between multiple tasks on the field
• [[Particle Detection]] for autonomous recollection
• [[Position Tracker]] to read encoders for position info and return to a set location after driving