Customizing StateMachineBuilder

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:

/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.