Do you want better robot code? There is a paradigm that my team has started to use, which makes your robot code a million times better. This article covers the creation of Finite State Machines, which will allow your team to program better and resilient robots.

This is going to in a slightly different format than normal, because this started off as a presentation. For this reason, the explanations under the slides are what would be said, and may differ from the format of normal posts.

Programming an FRC robot can be very different from normal applications. In this presentation, we will address how to think like a robot programmer, but not go in depth about the implementation. When implementation is mentioned, it will be based on a pre-2020 WPILib-based Java program, similar to the setup my team used last year. However, if you are trying to learn how to implement a command-subsystem structure, this is not the post.

By reading this presentation, you will learn how to write versatile, reliable, and scalable robot code for any robot that you may encounter while in FRC. This  method can be applied to other types of code, but the scope of the presentation will solely be for FIRST Robotics robot code.

This presentation will be broken up into three sections. First is the goal of robot code. Second is the basics of Finite State Machines, which are a tool we use to make better robot code. Finally we will cover the theoretical implementation of a Finite State Machine.

When developing robot code, it is important to understand the purpose of robot code. Our fundamental goal is to ensure that the robot reliably gets in the form that the driver wants. Even though we describe ourselves as programmers, programming is a tool that we use to achieve this end goal. Theoretically, if we could achieve this goal without code, we wouldn’t need code. However, in FIRST, code is the only viable option to achieve this end result.

With this new goal in mind, we need to ask ourselves: What is more important: the process or the end result?.

Based on our goal, our primary concern is the drivers, and our drivers don’t care how a robot gets to their desired form; they only care that it does. This is the same way that robot development works. Teams get a game on kickoff, and we start off by creating a strategy. When we create a strategy, no thought is given into how a robot will achieve the end result (scoring a point), we just assume that a robot will achieve the end result. We then work backwards to figure out what needs to happen for scoring to occur.

The idea that the end result defines the mechanism actions is one of the fundamental ideas of robot code. Our responsibility is to get the mechanism there, and everything else supports that idea.

It is important to have context for what caused my team to rethink our robot code.

In 2018, our drivers were responsible for completing robot actions to achieve the desired end goal. This is the complete opposite of robot code, but at the time, we felt that drivers should be the main people in control (see my post about competency for more information about our philosophy at the time).

Our elevator was moved manually with joysticks, and we didn’t even have a gravity compensation function in the beginning, so our operator had to apply a slight amount of power upwards (this was fixed later on) so that it would stay in place. In general, our robot was very hard to drive and our scoring was dreadfully slow.

In 2019 we redesigned around a button board, where the operator could push a button and the robot would go to the desired end state. Now the drivers didn’t have to worry about how to get there, especially since we had several subsystems that conflicted with each other (the cargo intake, when retracted, would intersect the path of travel for the manipulator).

The solution to our problem was in a concept called a Finite State Machine. A Finite State Machine is a well-tested code organization method that allows you organize and design your code prioritizing the end result.

To get started, it can be helpful to understand the vocabulary used with Finite State Machines. This makes it so that you can understand the linking between various concepts.

Given the vocabulary, why do you think it would be called a Finite State Machine?

The answer is simple, your code views your robot as a collection of machines, and each one can exist in one and only one of a finite number of states.

We use diagrams to help us understand what our robot needs to do and guide our development. In this diagram:

  • Circles represent a state that a machine can be in
  • Lines represent a transition between the states
  • Labels represent the condition that triggers a transition and the action that is taken

Now, think about what you notice about the coloring in the diagram.

Whenever a specific condition is met, the robot will transition to the end result state. Look at Condition A: it can trigger action ! if the machine is currently in stage one and action % if the robot is in stage three. However, even though the actions are very different, the end result is the same: State 2.

There are a four core concepts that help guide the development of a Finite State Machine.

One State At A Time

This concept is important for making sure that your robot code is manageable and easy to trace. With a Finite State Machine, a machine must exist in one and only one state at any given time. You can have multiple machines for multiple mechanisms, but if you have a robot with multiple states, you won’t be able to figure out which state to transition to.

End The Same Way

If I run a transition to a state, the mechanism should end the same way every time. If you want it to end slightly differently, add a new state. Your code should be based upon the idea of doing whatever is needed for the robot to end the same way, and any possible transitions between states should be accounted for.

Account for Failure

No robot is perfect. You can have unreliable sensor, brown-outs, field faults, or any other thing in a laundry-list of problems. What does this mean? Your robot code needs to be smart enough to account for these problems.

Almost every State Machine will have an error state. The robot should transition into the error state if there is a problem. While the details of an error state will differ based on the mechanism, there are two major ways to handle problems.

Global Good Practices

If there is an error, stop the robot.

Motor controllers will continue to send the same power until a different signal is sent. If anything bad happens, you want to make sure that the motors turn off immediately while the robot tries to correct.

What my team does in error conditions is set any affected motor controllers to (ControlMode.PercentOutput, 0). This line will shut off all control systems and feedback.

Self-Correct

Self-correction occurs when a robot attempts to autonomously correct form an error. It could include checking sensor values or re-homing. This usually should be done a couple of times before going into the second section.

Manual Control

Manual control should happen in bad situations (such as encoder failure) or when self-correct does not work. In this situation, it is the responsibility of the driver to know what to do to regain control or how to control the robot without use of the state machines. Our 2019 robot had a manual hand-held Logitech joystick that was solely a backup in the event of state machine failure.

Homing

In order to control your mechanisms, you need to know where they are. When a robot is restarted, relative encoders are set to zero. Absolute encoders don’t need to be homed but most mechanisms, like an elevator, need to be zeroed.

The way that homing works is that the mechanism goes to a known point (like the bottom of an elevator) where a limit switch is. When that limit switch is zeroed, set the encoder to zero. Now, we can use position control on the mechanism.

The last part of the Finite State Machine is implementing it. For this example, we will talk about a coin-operated turnstile, since it is an easy example of how state machines work.

We start off designing our state machine by asking these questions. For a coin-operated turnstile:

  • The turnstile can be unlocked or locked
  • It doesn’t interact or conflict with any other mechanisms
  • Homing is not needed since the machine will be locked on startup
  • There needs to be a coin detector and an encoder on the rotation arm
  • If something goes wrong, the machine should be locked

Now we need to lay out the diagram. For our states we have two states:

  • Locked
  • Unlocked

and two conditions:

  • Coin inserted
  • Person walks through

From this information, we can generate a diagram like below:

Diagram from Wikimedia Commons
Diagram from Wikimedia Commons

Now that we have our diagram, we can lay out our code.

Subsystem

Our actions for this would be SetToLocked and SetToUnlocked and would be a Command since there is only one action.

Our Turnstile subsystem would have an object property called Lock lock as well as an enum State and its related property State turnstileState.

Our enum would have two values, LOCKED and UNLOCKED:

public enum State {
    LOCKED, UNLOCKED
}

(see information about enums)

Our lockState is set to State.LOCKED in the constructor for the class, since we want it to be LOCKED by default.

SetToLocked

Set to locked has two roles: unlock the lock and update the state of the turnstile. The pseudo-code for our command’s initialize() function would call turnstile.lock.unlock() and turnstile.turnstileState = State.UNLOCKED && turnstile.lock.isUnlocked(). This command is triggered when a coin is put in to the machine.

The isFinished() method will return true once turnstile.turnstileState == State.UNLOCKED, and therefore exit the command.

SetToUnlocked

SetToUnlocked does the exact opposite of SetToLocked and is triggered once the person walks through.

This has been super theoretical, but I hope it helps you understand the basics of how a state machine works in concept.

There are several good apps for maintaining a diagram. I personally use Microsoft Visio, since it is an industry-standard tool. LibreOffice Draw is a free alternative that has a very similar feature set and also supports Linux. GraphViz is a text-based render application that works very well on Linux (though it has Windows and Mac support) and is very good for large diagrams, since it is based off of files.


Implementing State Machines – Team Specific Standards

This is not part of the main presentation since implementations may vary on a team-by-team basis. However, I will go over how my team implements state machines as of the 2020 season.

For many of these methods, you will need PID. If you don’t know what PID is, check out my blog post about it.

Position Controlling a Mechanism

When we need position control, it is highly advisable to use the ControlMode.MotionMagic on the Talon SRX (or equivalent on the Spark MAX). Motion Magic uses a linear motion profile to have smooth position control with ramping up and ramping down, which provides very accurate results.

When position controlling, it is VERY important to have a high resolution encoder. My team tries to use the highest resolution we possibly can, usually 2048 ticks per revolution.

Velocity Controlling a Mechanism

If we need a machine to have velocity control, we can use ControlMode.Velocity. Velocity control mode is good for continually-rotating mechanisms, such as a shooter or roller intake.

Command Layout

Our code should be laid out in the following way:

  • Send any motion in initialize() and set done to false
  • Create a variable done that can cause it to end manually
  • In isFinished() : return (condition || done)
  • In end() stop the motors

Laying Out the Command Groups

Command Groups should run all of the actions required to get something to the end result.

In Legacy Command Subsystem format (-2019), run all commands and have them immediately finish if they do not need to run (using the done variable). In the new Command Subsystem format (2020-), use conditional command groups to only run the necessary actions.

It can be very helpful to diagram the command transitions, in a manner similar to the one below, to figure out the logic before implementing.

From the 2019 Robot GitHub (Team100/2019-public)

Elevator or Arm States

When using an elevator, there should be two different states: the elevator level and the elevator action. We used the action as our primary state tracker (referred to in Commands)and our level as the one to help guide our actions.

Our actions are:

public enum States{
    AT_SETPOINT,MOVE_TO_SETPOINT,HOMING,TELEOP
}

Use Real-World Units

It can be much easier to represent set-points in real-world units and then convert to native values. One benefit is that non-programmers can easily convert. The other benefit is that it is very easy to get approximate values and then fine-tune later. An example of our code is below:

    /**
     * Convert elevator inches to native encoder ticks
     * @param inches
     * @return  the height in encoder ticks
     */
    public static int convertInchesToTicks(double inches){
        if(inches > Constants.ELEVATOR_MAX_HEIGHT_IN_INCHES){
            System.out.println("INCHES EXCEED MAX");
            return (int)(Constants.ELEVATOR_MAX_HEIGHT_IN_INCHES - Constants.ELEVATOR_START_HEIGHT_IN_INCHES) * Constants.ELEVATOR_INCH_TO_ENCODER_CONVERSION_FACTION;
        }
        return (int)((inches- Constants.ELEVATOR_START_HEIGHT_IN_INCHES - Constants.ELEVATOR_CENTERLINE_TOP_BAR_DISTANCE) * Constants.ELEVATOR_INCH_TO_ENCODER_CONVERSION_FACTION );
    }

Neutral Default Command

It can be a very good idea to have a default command for your subsystem that does nothing. This makes it so that the system will remain where it was before. For our 2019 robot, our default command was completely empty for the elevator, so that the elevator would just stay wherever it was before.

Use a Button Board

Using a Button Board can be a huge help for the drivers. We use a TI micro controller that acts as a USB joystick, and a physical box mounted to our driver-station box with an array of buttons.

As you can see in the bottom left, we have a switch that goes between Auto and Manual control. Manual control shuts off the FSM, and Auto restarts it. This was very helpful during matches when things went wrong (like needing to velcro up our cargo intake because we didn’t pressurize) to flip the switch a couple of times and regain control of the robot state machine.

Looking for More Information About Team Standards?

Our 2019 code is the basis for team standards, though standards are always subject to change.

As always, we should not let our code be stagnant, so I encourage anyone on Team 100 or other teams to work on improving the Finite State Machine system that we have.

0 Comments

Cancel

This site uses Akismet to reduce spam. Learn how your comment data is processed.