GeekMyBot: FTC Blocks State Machine Example (Part 1)

Blocks Programming State Machine Example (Part 1)

This is example that shows how to program behaviors using a state machine for a robot using the block programming language. Block programming language is one of the options for the FIRST Tech Challenge Robotics competition in 2017-18.

Why use a state machine? The complex behaviors of a robot can be broken down into a set of simple behaviors. For example drive forward 6 inches. A more complex behavior happens when simple behaviors are chained together. For example:
1- Drive forward 6 inches then
2- Turn 45 degrees to the right then
3- Backup 2 inches then
4- Turn 45 degrees to the left

Instead of sequentially moving from simple behavior to simple behavior, events can trigger the movement between these behaviors in any order. Behaviors may also happen simultaneously. A State machine is one way to accomplish these more complex behaviors. To read more about state machines see my article titled "State Machines" the Wikipedia article "Finite-statemachine".

The hardware for the stat machine example consists of a square robot with 2 drive motors named m1 and m2. The encoders are wired up to the motor controllers.

The state machine op mode begins by reversing a motor direction. This results in a positive command on both m1 and m2 resulting in a forward motion on the robot. The motor encoders mode is set to "Run_USING_ENCODER". This results in a command to the motor setting a velocity rather than the power of the motor. Think of this like the speed control on an automobile.

Next the distance, speed, and motor controls are initialized. See diagram 1below. These function calls are covered in more detail in a later article.

Next the variable State is initialized to 0. The variable State will determine which state is active in the state machine. See diagram 1below. I will describe this in more detail later in the article.

The variable timerOld is initialized with the current system time. This will be used to measure how long it takes to run through each loop of the op mode. See diagram 1below.

Lastly the call StateMachineExample waits for the start button to be pressed on the controller android phone. See diagram 1below.

Diagram 1

The state machine is run inside a while loop. The while loop is exited as soon as stop is selected on the controller android phone. See Diagram 2. IMPORTANT: The design of this state machine only works if this while loop is the only while loop in the code. Putting a while loop inside of this while loop will prevent the state machine from working.

CalculateDistanceAndSpeed is a function call that uses the encoder to measure how far the robot has traveled, the direction the robot is pointed, and the speed that the robot is traveling at. See Diagram 2.

Diagram 2

The next set of software calculates the events that are used to transition from one state to the next state. See Diagram 3.

The Game pad button A, B, X, and Y are used to transition the statemachine to state 0, 1, 2, and 3. It does this by setting the variable called "State" to an integer number of 0, 1, 2, or 3. Each of these transitions also resets the distance, speed, and motor controls. See Diagram 3 below.

Diagram 3

The state 0 drives the robot using arcade mode on the joystick. An if statement only runs this software if the current value of State is 0. This State value is set using an event. See Diagram 4.

Notice that the gamepad Joystick values are used to calculate motor commands. The motor commands are stored inside of a variable. These variables will be used to command a motor speed at the end of the while loop. See Diagram 4.

Diagram 4

State 1 drives the robot forward 15 inches and stops. An if statement only runs this software if the current value of State is 1. This State value of 1 is set using an event. See Diagram 5.

State 1 works by calling the "DriveUntilDistanceReached" Function call. This function sets m1Cmd and m2Cmd to move the robot and it returns a true value when the distance the robot moves is 15 inches or greater. The function return value is used to trigger an event. If the event happens (a true value is returned from "DriveUntilDistanceReached") the state machine is moved from state 1 back to state 0 and it resets the motor controls.

See Diagram 5.

Diagram 5

The next two states, state 2 and state 3 are used to turn the robot to the right by 90 degrees or to the left by 90 degrees. When the distance of 90 degrees is reached, the state machine is transitioned to the state of 0. See Diagram 6.

Diagram 6

Finally, the motor commands m1Cmd and m2Cmds are used to command motor movement. Because the encoders are enabled, the power command actually controls the motor speed. See Diagram 7.