Take control of your robot's direction using a Gyro in Driver

Geek my driving

My robot turns when I drive it in autonomous or in driver mode. What can I do to help it drive straight?

One answer to this problem is to use the gyro to help control the robot direction. This article describes how to do this. The language I will use is the Android Block language.

Robot Hardware

Here is a picture of the robot that I will control:

The key parts you will need on your robot is two drive motors and the integrating Gyro.

Robot Software

The block programing language to initialize the software is shown in the diagram below.

The motors are setup to command a speed using the RUN_USING_ENCODER mode. This command is optional. Steering using the gyro works with or without encoders.

The gyro needs to be calibrated in the initialize section of the software. When calibration is running it is important not to move the robot.

The variables turnRate, turnRateOld1 and turnRateOld2 are initialized to zero. These variables will be used later on.

The block programming language to calculate the turn command and to drive the motors is shown in the diagram below.

One while loop runs as long as the drive opmod is active.

The calculations used to control the direction are described here:

• turnRate - The gyro block reads in the gyro rate and stores it in the variable turnRate.
• turnCmd - The turnCmd variable is the commanded speed the robot should turn. This is calculated by subtracting the desired turn rate (the left stick x position LeftStickX) from the measured turn rate (turnRate). In an ideal world the error between these two signals is zero. To control the robot motors to move the robot at this rate this error term is multiplied by a gain. The higher the gain number, the smaller the error. This type of control is called a proportional controller.
• What is the purpose of turnRateOld1 and turnRateOld2? This is a very simple way of reducing the amount of noise. Noise results in twitchy motor. The filter works by averaging the last 3 samples of the gyro turn rate. An example of the filter is shown below
•  

• turnRateOld1 / turnRateOld2 - Store off the past values of the turn rate.
• m1Cmd - The motor 1 command is the turn command subtracted from the speed command (The left stick y position LeftStickY).
• m2Cmd - The motor 2 command is the turn command added to the the speed command (The left stick y position LeftStickY).
• m1 / m2 - It is important to limit the size of the commands sent into the motor. The constraint block is set to limit the command to between -1 and 1.

Tuning the proportional controller.

STEP 1  - Adjust the gyro turn rate gain. The gyro turn rate number is 80000 in the example above.  (turnCmd = js + (turnRate + turnRateOld1 + turnRateOld2)/8000). To set this number send turnCmd out on telemetry. When your robot is running, pick it up and rotate it as fast as you would like the robot to turn. The number you see in telemetry should be between 0.3 and 0.7 (or -0.3 and -0.7).  Adjust the number 80000 until you see a turnCmd number in this range.


STEP 2  - Adjust the proportional control gain.  In the above example, the proportional gain is 1. This seems to work well for most robots.  If you desire better control, increase the gain number until the robot begins to oscillate left and right. Decrease the number until the oscillations stop. In the following example, the gain was increased to 3.

FAQ - My controller does not work at all. When I start driving the robot just turns around in a circle and never stops. Why is this?

Answer - The controller needs to generate an error signal. If the gyro rate is commanding the same direction as the joystick, a positive feedback will happen. A positive feedback signal results in an unstable system. We need negative feedback to keep the system stable. To fix this, just change the sign of the turnCmd calculation. Change (turnCmd = js + (turnRate + turnRateOld1 + turnRateOld2)/8000) to (turnCmd = js - (turnRate + turnRateOld1 + turnRateOld2)/8000). Notice the change in the sign after js from + to -. In the following example, the sign was changed to a negative sign.

FAQ - My controller is sluggish. It does not seem to track very well. How do I fix it?
Answer - This is the result of a proportional gain that is to small. Change (turnCmd = js + (turnRate + turnRateOld1 + turnRateOld2)/8000) to (turnCmd = (js + (turnRate + turnRateOld1 + turnRateOld2)/8000)*3). Adjust the gain (3 in the previous example) to a larger number to have tighter control. Adjust the gain to a smaller number if the control begins to oscillate back and forth.

FAQ - My robot does not turn very fast or it turns way to fast. How do I adjust the turn speed?
Answer - This is the result the gain used to match the gyro turn rate with the joystick. To set this number send turnCmd out on telemetry. When your robot is running, pick it up and rotate it as fast as you would like the robot to turn. The number you see in telemetry should be between 0.3 and 0.7.
In the calculation  (turnCmd = js + (turnRate + turnRateOld1 + turnRateOld2)/8000) adjust the number 80000 up and down until you get a telemetry number in this range.

FAQ - My controller oscillates back and forth. What should I do?
Answer - the is is a result of a proportional gain that is to large. The proportional gain is the number 3 in the following example: (turnCmd = (js + (turnRate + turnRateOld1 + turnRateOld2)/8000)*3). Keep reducing this number until the oscillation goes away. For example, 2, 1, .5, .25, .1.

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.

Diagram 7

Telemetry is used to send out debugging information such as the m1 distance, m1 command, active state, and the status of isStalled. See Diagram 8.

Diagram 8

Lastly the while loop is ended. Remember that there should not be additional while loops inside of this state machine.  See Diagram 9.