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.

FTC Robot Behavior Model (Part 5 - OpMode)

This is part 5 of a 5 part series on a robot behavior framework.

Behaviors are connected together with events to form more complex behaviors. In this simple example, pressing the y button on the joystick will trigger a change in the active behavior from driving the robot with an an arcade style input to driving the robot with a tank style input.


The OpMode software does the following:

It begins by setting up the behavior framework in the H1TeleOp_Js2() constructor or the opmode init() method as follows:

// ConstructorBehaviors_Joystick be;
GlobalData gd;
public H1TeleOp_Js2(){
    // Setup the behavior model    gd = new GlobalData();
    be = new Behaviors_Joystick(gd);
}

In our example, the behaviors framework is called Behaviors_Joystick(). GlobalData is needed to pass information back and forth in the loop() method for the opmode as follows:

// Run periodically when sensors are read@Overridepublic void loop() {
    // Pass Joystick to the behaviors    gd.jsLeftY = gamepad1.left_stick_y;
    gd.jsRightY = gamepad1.right_stick_y;
    gd.jsLeftX = -gamepad1.left_stick_x;
    gd.buttonX = gamepad1.x;
    gd.buttonY = gamepad1.y;

    // Pass time to the behaviors    gd.deltaTime = (float)time - gd.time;
    gd.time=(float)time;

    // Run the behaviors    be.onLoop();

    // write the values to the motors    motorRight.setPower(gd.rightCmd);
    motorLeft.setPower(gd.leftCmd);

    telemetry.addData("_Behavior", be.getActiveBehavior().getName());
    telemetry.addData("Power Left", Float.toString(gd.leftCmd));
    telemetry.addData("Power Right", Float.toString(gd.rightCmd));
    telemetry.addData("Left Js Y", Float.toString(gd.jsLeftY));
    telemetry.addData("Right Js Y", Float.toString(gd.jsLeftY));

    //DbgLog.msg("Run Mode");}

The method onLoop() calls the behavior framework. When the robot first starts up, it will begin by calling the Behavior_JsArcade behavior.

For full copies of this software, see: https://github.com/dlacres/GeekMyBot.git 

This github repository has the following behaviors defined:

• Behavior_Forward - Drive forward
• Behavior_Heading - Use the dpad and the gyro to turn the robot (demonstrates a simple PID control law)
• Behavior_IrTurn - Turn toward the IR Beacon. Move forward and backward to match the signal strength.
• Behavior_JsArcade - Drive using Arcade mode
• Behavior_JsGyro - Drive using Arcade mode. Correct for drift with the gyro.
• Behavior_JsLookup - Drive using Arcade mode. Provide better low speed turns using a lookup table to shape the commands.
• Behavior_JsTank - Drive using Tank mode.

This github repository also has the following events defined:

• Event_ButtonX - Transition on the rising edge of a X button push
• Event_ButtonY - Transition on the rising edge of a Y button push
• Event_Time - Transition after a period of time

 

If you use this behavior framework, I ask that you share the love. Print out a copy of the GeekMyBot logo and put it on your robot along with the http://geekmybot.blogspot.com/ URL address. Share this Blog with your friends.

If you develop a behavior that you are particularly proud of, I ask that you add a comment to this article and share your behavior along with a link to your software.

FTC Robot Behavior Model (Part 4 - Global Data)

This is part 4 of a 5 part series on a robot behavior framework.

Behaviors are connected together with events to form more complex behaviors. In this simple example, pressing the y button on the joystick will trigger a change in the active behavior from driving the robot with an an arcade style input to driving the robot with a tank style input.


This article describes how information is passed from the joystick, motors, and sensors into the behavior framework. The GlobalData class is a buffer between the robot opmode and the behavior classes. If we share behaviors and events as a community, the global data class lets us re-map the I/O between names used in the opmode and names used by different behaviors. Although this is not the most elegant solution, it is simple and fast to implement. Get and Set methods can be used instead of public data if desired.


There is no template or interface for the Global Data. Just create a class called GlobalData and define the data your behavior model needs and produces. Here is an example of the GlobalData class I used in my example:

package com.qualcomm.ftcrobotcontroller.geekmybot;

/** * Created by David Lempia on 8/20/2016. */public class GlobalData {
    public GlobalData(){
    }
    // Gyro    public float headingDot=0.0f, heading=0.0f, headingCmd=0.0f;

    // Motors    public float rightCmd=0.0f, leftCmd=0.0f, rightCmd1=0.0f, leftCmd1=0.0f;

    // Joystick    public float jsLeftX, jsLeftY, jsRightX, jsRightY;
    //public float throttleJs=0.0f, directionJs=0.0f;    public boolean buttonX, buttonY;
    public boolean dpadRt, dpadLt, dpadUp, dpadDn;

    // Time    public float time,enterTime;
    public float deltaTime=0.0f;

    // IR Seeker    public double irAngle,irStrength;
    public boolean irDetected;

    // Debug    public float debugFloat1=0.0f, debugFloat2=0.0f;
    public int debugInt1, debugInt2;
    public double debugDouble1, debugDouble2;
    public boolean debugBoolean1, debugBoolean2;
}

For full copies of this software, see: https://github.com/dlacres/GeekMyBot.git 
 
If you use this behavior framework, I ask that you share the love. 
Print out a copy of the GeekMyBot logo and put it on your robot along 
with the http://geekmybot.blogspot.com/ URL address. Share this Blog 
with your friends.

If you develop an event that you are 
particularly proud of, I ask that you add a comment to this article and 
share your behavior along with a link to your software.

FTC Robot Behavior Model (Part 3 - Behaviors)

This is part 3 of a 5 part series on on a robot behavior framework.



The behavior model is tied together in in the Behaviors class. The Behaviors class creates new instances of each behavior and event. It also defines which behavior is the active behavior to start with. Finally it connects the events to the behaviors

The Behaviors class implements the Behaviors interface. The methods onLoop() and getActiveBehavior() are used by Behaviors. Each method has a purpose in the framework as follows:

 - onLoop() runs each time the robots's opmode is run.
 - getActiveBehavior() is used by the framework to get and run the onLoop() for the active behavior.

In this example, the behaviors class will start running the Behavior_JsArcade class. This class drives the robot using the left joystick in arcade mode. When the Y joystick button is pressed, the behavior changes to the Behavior_JsTank class. This class drives the robot using the left and right joystick in tank mode. When the joystick Y button is pressed again. the behavior transitions back to the Behavior_JsArchade class. The following picture depicts this behavior:

To use Behaviors copy the Behaviors_Template.java shown below into a new class name. I start the name of all of my Behaviors classes with the naming convention Behaviors_XX where XX is the name of my Behaviors. Here is a copy of the template:

package com.qualcomm.ftcrobotcontroller.geekmybot;

import java.util.List;

/** * Created by David Lempia on 7/20/2015. */public class Behaviors_Template {

    // Public SM Inputs and Outputs
    // Declare the states and events
    // Add the states and events used in this state machine    public Behaviors_Template(GlobalData gd){
        // Create the states - Input is the name of the state for debug        //Behavior behavior?? = new Behavior_??(  gd, "?? Behavior");
        // Create the events - Behavior From, Event, Behavior To        //behavior??.addEvent( new Event_ButtonY(gd, behavior??));
        // The start state for the state machine        //activeBehavior = ??    }

    // Do not change this. This stays constant    private Behavior activeBehavior;
    public void onLoop(){

        List<Event> el = activeBehavior.getEventList();

        //Log.i("info", "Made it to 1");        for (Event el1 : el){
            //Log.i("info", "Made it to 2");            if (el1.isTriggered()){
                // If the event happened, run exit, step, and entry code                activeBehavior.onExit();
                el1.onTransition();

                // Update the active state                activeBehavior = el1.getToBehavior();
                activeBehavior.onEntry();
                break; // Do not check the rest of the transitions            }
        }
        // Run active state        activeBehavior.onLoop();
    }
    public Behavior getActiveBehavior(){
        return(this.activeBehavior);
    }
}

Begin by creating a new set of behaviors  in the Behaviors_Joystick2 creator as follows:

// Create the behaviors - Input is the name of the behavior for debugBehavior behaviorJsArcade = new Behavior_JsArcade(gd, "Js Arcade");
Behavior behaviorJsTank = new Behavior_JsTank(gd, "Js Tank");

Right after this software in the same module, create a set of events and define their transitions as follows:

// Create the events (lines)behaviorJsArcade.addEvent( new Event_ButtonY(gd, behaviorJsTank));
behaviorJsTank.addEvent( new Event_ButtonY( gd, behaviorJsArcade));

Finally, define the initial active behavior (The behavior the robot starts in) as follows:

// The start state for the state machineactiveBehavior = behaviorJsArcade;

The resulting Behaviors_Joystick2 class is shown below:

package com.qualcomm.ftcrobotcontroller.geekmybot;

import java.util.List;

/** * Created by David Lempia on 7/20/2015. */public class Behaviors_Joystick2 implements Behaviors {

    // Add the behaviors and events    Behavior activeBehavior;
    public Behaviors_Joystick2(GlobalData gd){

        // Create the behaviors - Input is the name of the behavior for debug        Behavior behaviorJsArcade = new Behavior_JsArcade(gd, "Js Arcade");
        Behavior behaviorJsTank = new Behavior_JsTank(gd, "Js Tank");

        // Create the events (lines)        behaviorJsArcade.addEvent( new Event_ButtonY(gd, behaviorJsTank));
        behaviorJsTank.addEvent( new Event_ButtonY( gd, behaviorJsArcade));

        // The start state for the state machine        activeBehavior = behaviorJsArcade;
    }

    // Do not change this. This stays constant    public void onLoop(){

        List<Event> el = activeBehavior.getEventList();

        //Log.i("info", "Made it to 1");        for (Event el1 : el){
            //Log.i("info", "Made it to 2");            if (el1.isTriggered()){
                // If the event happened, run exit, step, and entry code                activeBehavior.onExit();
                el1.onTransition();

                // Update the active state                activeBehavior = el1.getToBehavior();
                activeBehavior.onEntry();
                break; // Do not check the rest of the transitions            }
        }
        // Run active state        activeBehavior.onLoop();
    }
    public Behavior getActiveBehavior(){
        return(this.activeBehavior);
    }
}

For full copies of this software, see: https://github.com/dlacres/GeekMyBot.git

If you use this behavior framework, I ask that you share the love. Print out a copy of the GeekMyBot logo and put it on your robot along with the http://geekmybot.blogspot.com/ URL address. Share this Blog with your friends.

If you develop an event that you are particularly proud of, I ask that you add a comment to this article and share your behavior along with a link to your software.

FTC Robot Behavior Model (Part 2 - Events)

This is part 2 of a 5 part series on on a robot behavior framework.


Behaviors are connected together with events to form more complex behaviors. In this simple example, pressing the y button on the joystick will trigger a change in the active behavior from driving the robot with an an arcade style input to driving the robot with a tank style input.

The event class implements the event interface. The methods onTransition(), isTriggered(), and getToBehavior() are used by the behavior. Each method has a purpose in the framework as follows:

 - onTransition() runs only when the event is triggered resulting in a transition between behaviors. The order of transitions are set in the behaviors class. This will be covered in the part 3.
 - isTriggered() returns a boolean value. If the Boolean is True, the transition occurs.
 - getToBehavior() is used by the framework when it requests the destination behavior.


To use this behavior, copy the Event_Template shown below into a new class name. I start the name of all of my event classes with the name Event_xx. This naming convention helps when our team shares events for use in different programs. Here is a copy of the template:

package com.qualcomm.ftcrobotcontroller.geekmybot;

/** * Created by David Lempia on 8/2/2015. * * Copy this template and rename it with the name of your event. i.e. Event_ButtonX. * Inputs and Outputs are put in the Behaviors_xx file. Access them using gd.xx * Add the event to the Behaviors_xx file. * Add inputs and outputs to the Behaviors_xx file. * Pass parameters in through the Event_Template Constructor. */public class Event_Template implements Event {
    private Behavior toBehavior;

    // Returning true triggers the event    // Pass variables    public boolean isTriggered(){
        return(false);
    }

    // This runs if the event is triggered    public void onTransition(){

    }

    // Change Behaviors_XX to the name of your behaviors class    // Add parameters as needed    GlobalData gd;
    Event_Template(GlobalData gd, Behavior toBehavior){
        this.gd=gd;
        this.toBehavior = toBehavior;
    }

    // No change needed    public Behavior getToBehavior() {
        return(this.toBehavior);
    }
}

To create an event that transitions whenever the Y joystick button is pressed, add a new RiseEdgeTrigger to the constructor. Check for a button press in the isTriggered() method. Here is what the final sofware looks like for the Event_ButtonY.java class:

package com.qualcomm.ftcrobotcontroller.geekmybot;

/** * Created by David Lempia on 8/2/2015. */public class Event_ButtonY implements Event {

    // Returning true triggers the event    public boolean isTriggered(){
        boolean button_pressed = pressed.calculate(gd.buttonY);
        return(button_pressed);
    }

    // This runs if the event is triggered    public void onTransition(){

    }

    // No change needed    RiseEdgeTrigger pressed;
    private Behavior toBehavior;
    GlobalData gd;
    Event_ButtonY(GlobalData gd, Behavior toBehavior){
        this.gd = gd;
        this.toBehavior = toBehavior;
        pressed = new RiseEdgeTrigger();
    }

    // No change needed    public Behavior getToBehavior() {
        return(this.toBehavior);
    }
}

Here is the software for the RiseEdgeTrigger.java class:

package com.qualcomm.ftcrobotcontroller.geekmybot;

/** * Created by David Lempia on 1/17/2016. */public class RiseEdgeTrigger {
    private boolean z1=false;
    private boolean out=false;

    public boolean calculate(boolean in){

        if (in & !this.z1) this.out=true;
        else this.out=false;

        this.z1=in;

        return(this.out);
    }
}

For full copies of this software, see: https://github.com/dlacres/GeekMyBot.git 

If you use this behavior framework, I ask that you share the love. Print out a copy of the GeekMyBot logo and put it on your robot along with the http://geekmybot.blogspot.com/ URL address. Share this Blog with your friends.

If you develop an event that you are particularly proud of, I ask that you add a comment to this article and share your behavior along with a link to your software.

FTC Robot Behavior Model (Part 0)

This article is written for FTC robots using the Android Developer Studio. It describes a behavior model similar to a state machine that can be used to do very interesting things with a robot. For example, the behavior model can control a complex motion on an arm, autonomously drive the robot along a path, or interrupt a motion when a sensor detects an interfering robot. In this example I picked a very simple behavior to demonstrate how to use the behavior model. See the image below:







Pressing the Y button on the joystick will transition the drive mode of the robot from Arcade to Tank and back again.

If you use this behavior framework, I ask that you share the love. Print out a copy of the GeekMyBot logo and put it on your robot along with the http://geekmybot.blogspot.com/ URL address. Share this Blog with your friends.

If you develop a behavior that you are particularly proud of, I ask that you add a comment to this article and share your behavior along with a link to your software.

Robots are interesting when they perform a set of different behaviors. For example one behavior may be to drive using the joystick in tank mode. Another behavior may be to drive using the joystick in arcade mode.

Wouldn't it be nice if:

• I could build a library of simple behaviors. When I need one, I just add it from the library.
• I could build complicated behaviors by transitioning from one simple behavior to another behavior when an interesting event takes place.
• I could re-use the behaviors in many different applications.

This article describes a simple robot behavior framework (set of Java Classes) that makes constructing behaviors and transitioning from one behavior to another easy. The article is written in 5 different parts as follows:

Part 1 - Behavior
Part 2 - Events
Part 3 - Behaviors
Part 4 - Global Data
Part 5 - OpModes


These articles are written for a two wheel drive robot that is typically used in FTC competitions. Here is a picture of the robot used in this example:

The robot has two drive motors and wheels, a left drive and a right drive. There is one omni-wheel in the back. The electronics are mounted vertically. From left to right in the picture above, the electronics are an android phone, The power distribution (the one shown is home-built using a USB hub and a 5 volt regulator), a Core Legacy Module, a Core Motor Controller, and a Core Device Interface. There is an IR seeker V3 and an integrating gyro sensor plugged into the Core Device Interface.

All software can be downloaded from - https://github.com/dlacres/GeekMyBot.git