LAB 9

MOBILE ROBOT CONTROL

1. LAB OBJECTIVE

The objective of this lab is to design and implement a motion control system for a mobile robot.

The developed controller has to ensure that the robot can follow a designed trajectory while

avoiding obstacles.

2. BACKGROUND

The National Instruments (NI) Robotics Starter Kit 1.0 is a mobile robot platform that comes

equipped with sensors, motors, and a NI Single-Board RIO for embedded control. NI LabVIEW

graphical programming and the LabVIEW Robotics module can be used for programming the

mobile robot.

Figure 1: NI Robotics Starter Kit

2.1 Robot Components

The NI Robotics Starter Kit uses a NI Single-Board RIO 9631 embedded control platform and an

ultrasonic range finder. The Single-Board RIO controller integrates a real-time processor,

reconfigurable field-programmable gate array (FPGA), and analog and digital input/output (I/O)

on a single board. It is powered by both NI LabVIEW Real-Time and FPGA technologies. The

built-in analog and digital I/O can be expanded using C Series modules.

The robot has two DC servomotors and 4 wheels. The DC motors are positioned between the

front and rear wheels on each side and connected via a 2-1 gear train to both wheels. Each motor

has a 400-tick encoder. Thus, the motor for each side (left or right) can be controlled

independently. The steering method for this wheel configuration is called skid-steer.

Figure 2: Robot Components

The robot is equipped with a Parallax PING))) ultrasonic sensor that detects objects by emitting a

short ultrasonic burst and then listening for the echo. Under the control of a host microcontroller,

the sonar sensor emits a short 40 kHz (ultrasonic) burst. This burst travels through the air at

about 1130 feet per second, hits an object, and then bounces back to the sensor. The PING)))

sensor provides an output pulse to the host that terminates when the echo is detected; hence, the

width of this pulse corresponds to the distance to the target. This sensor can sense obstacles in a

range from 2 cm to 3 m. Moreover, the ultrasonic sensor is installed on a stepper motor. Thus,

the ultrasonic sensor can rotate from to degrees. (By rotating the ultrasonic sensor,

objects around the robot can be detected.)

The coordinate system of the robot is defined by Figure 3.

Figure 3: Coordinate System of the Robot

2.2 National Instruments Single Board RIO

The NI sbRIO-9631 embedded control and acquisition device (see Figure 4) integrates a real-

time processor, a user-reconfigurable field-programmable gate array (FPGA), and I/O on a single

printed circuit board (PCB). It features a 266 MHz industrial processor, a 1M gate Xilinx Spartan

FPGA, 110 3.3 V (5 V tolerant/TTL compatible) digital I/O lines, 32 single-ended/16 differential

16-bit analog input channels at 250 kS/s, and four 16-bit analog output channels at 100 kS/s. It

also has three connectors for expansion I/O using board-level NI C Series I/O modules. The

sbRIO-9631 offers a -20 to 55 °C operating temperature range along with a 19 to 30 VDC power

supply input range. It provides 64 MB of DRAM for embedded operation and 128 MB of

nonvolatile memory for storing programs and data logging.

This device features a built-in 10/100 Mbits/s Ethernet port that can be used to conduct

programmatic communication over the network and host built-in Web (HTTP) and file (FTP)

servers. The RS232 serial port can be used to control peripheral devices.

Figure 4: sbRIO-9631

2.3 Robot Control

The NI sbRIO-9631, single board RIO, is programmed by using NI LabVIEW software. Using a

LabVIEW program developed for this lab, the robot can be programmed with high-level

programming. Using the program provided, the robot and the controller transmit control signals

and data at a frequency of 100 Hz.

Figure 5: Robot Connection

For simplification, the two drive motors of the robot are programmed to operate at the same

speed in either the same or opposite direction. Accordingly, the two possible motion modes of

the robot are translation and rotation. The translation mode is controlled by sending the

“translate” command ( ). This command makes the robot left wheels and right wheels

rotate at the designated speed in rad/s ( ), forward ( ) or backward ( ). The

rotation mode is controlled by sending the “rotate” command ( ). This makes the robot

left wheels and right wheels rotate in different directions at the designated speed in rad/s ( ), counter clockwise ( ) or clockwise ( ). Lastly, the “stop” command ( ) is

used to stop the robot motors. These commands have to be sent to the sbRIO-9631 during real-

time control of the robot. The program to communicate with the robot processor is discussed in

section 3.

Table 1: Robot Commands

Command Description

Name

Stop 0 Stop motors (irrespective of specified speed, )

Translate 1 Rotate left and right wheels in same direction at specified speed, , in rad/s

Forward:

Backward:

Rotate 2 Rotate left and right wheels in different directions at specified speed, , in rad/s

Counter Clockwise:

Clockwise:

Note: When the robot is in the translation or rotation mode, the robot should be stopped before

switching to another mode. If the robot is switched between these modes while the wheels are

still in motion, an error in robot motion can occur.

2.4 System Model

The System Model with default parameters is as follows:

Figure 6: Block Diagram of Robot Motor

Figure 6 shows an open loop diagram of the motor. Raw encoder data of the wheels’ angular

positions are used to approximately compute the robot position. The position error increases as

the robot translates or rotates because of wheel slip. However, we can rely on these position

estimates if the robot moves within a short range (less than 6-8 meters). An approximation of the

robot velocity can be obtained by differentiating the encoder data. Thus, it is possible to estimate

both robot velocity and position. This estimation has been done for you, and these variables are

made available when controlling the robot motion.

3. ROBOT PROGRAMMING

3.1 Overview of LabVIEW Block Diagram

In this lab, you will program the National Instrument, sbIO-9631, a microcomputer, to control

the robot. The sbRIO-9631 is programmed using LabVIEW 2012. LabVIEW uses block

diagrams to implement a real-time program. Two basic programs have been provided to manage

the transfer of both commands and data to and from the robot. The first program, “Robot -

Manual Control” allows the robot motion to be controlled manually via user inputs on the

keyboard. The second program, “Robot - Formula Node Control”, relies on a formula node block

to control what commands are sent to the robot. The formula node allows you to perform

complicated mathematical operations and control using the C programming language. You are

encouraged to scan the help section for this block (see Appendix 10.A.2) and carefully program

the controller. (Tip: If the C code has errors, the Run button used to start the program will change

from to )

When you first open any LabVIEW program, you will see two windows, the front panel window

and a block diagram window. The front panel window allows you to monitor output variables

and set input parameters. The block diagram window displays the actual program written by

graphical programming and contains the code for controlling the robot.

Figure 7: Front Panel Window

This front panel window shows all the information obtained from the robot.

Plots:

o Sonar Plots: Time series sonar range measurements (filtered and unfiltered)

o Position Plots: Time series , , and data

o Trajectory Plot: Time series robot position and current heading

Data:

o Position: , , and with reference to starting location

o Velocity: Robot translational/rotational velocity and individual wheel velocities

o Encoder: Left/right encoder reading and calculated feedback translation/rotation

o Sonar: Sonar orientation and range measurements (filtered and unfiltered)

o Control: Commands currently being sent to the robot

Figure 8: Block Diagram Window

Figure 9: Block Diagram Primary Control Loop

Figure 8 shows the block diagram corresponding to the front panel provided in Figure 7. This

contains all code that is used for controlling the robot. The code is composed of several blocks

diagrams and sub-block diagrams (“sub-vi’s”). The primary control of the robot is contained

within the main timed while loop shown in Figure 9. Each sub-block diagram has its own sub

front panel and sub block diagram windows. These blocks are already prepared and their

programs do not have to be changed by you. However, you should make yourself familiar with

their inputs and outputs to understand how the block diagram works. Table A3 in the appendix

presents the description of the sub-block diagrams.

3.2 LabVIEW Formula Node

The most important element in the block diagram for this lab is the formula node block. An

example formula node block used to program the robot to track a reference sine wave input can

be seen in Figure 10.

Figure 10: Formula Node Block

The variables in blue and orange rectangles on the left side of the formula node are input

variables to the formula node. They have already been declared in the Labview program and can

be used directly. Similarly, the variables on the right side are pre-defined output variables. These

variables are what you will use to send the desired commands to the robot. Table 2 provides a

summary of all variables that have already been pre-defined in the program for you to use in the

formula node block. In addition to these variables, you may declare any new variable you want

using standard C syntax. For instance, in the example formula node provided in Figure 10 the

variables “f” and “t” needed to create the reference sine wave have been declared as temporary

float variables within the formula node.

Table 2 Pre-defined Formula Node Variables

Variable Data Type Description

X Double Robot X Position (in)

Y Double Robot Y Position (in)

Th Double Robot Th Position (in)

In2Cnt Double Encoder counts per inch of robot translation

Deg2Cnt Double Encoder counts per degree of robot rotation

FT Double Feedback translation (counts): feedback signal for translation

FR Double Feedback rotation (counts): feedback signal for rotation

sonar Double Filtered sonar reading (in)

count Int Current iteration (cycles)

delay Int Initial delay time (cycles)

Ts Double Sampling period (s)

state Int State (case) of the state machine program

reset Int Command to reset encoder (0 = do not reset, 1 = reset)

cmd Int Robot command (0 = stop, 1 = translation, 2 = rotation)

sonarAngle Double Sonar angle (deg)

vel Double Commanded velocity (rad/s)

error Double Current error

previous_error Double Error from previous loop

error_integral Double Error integral

error_derivative Double Error derivative

iR Int Rotation counter

All formula node output variables are fed back into the formula node as inputs using “shift

registers”, . In LabVIEW, this directs the program to store the variable so that it can be

used during the next loop iteration. Any variable declared in the formula node that is not wired in

this way will be removed from memory after finishing each cycle of the loop. New “shift

registers” can be added by right-clicking on either the left or right wall of the while loop.

Similarly, new input or output variables can be added by right-clicking on the wall of the

formula node.

3.3 LabVIEW Program Sequence Overview

1. Select the robot

2. Establish a connection and initialize the selected robot

3. Control the robot

a. Gather sensor data

i. Read the robot encoders

ii. Measure and filter the robot sonar range

b. Select the robot command and compute the desired velocity based on the encoder and

sonar data in the Formula Node block

c. Send the commands to the robot

i. Command the desired robot velocity

ii. Command the desired sonar angle

d. Reset the encoders if necessary

e. Reset the robot position if “RESET” button is pressed

f. Update the robot position (and plots)

4. Repeat Step 3 until the “STOP ROBOT” button is pressed

5. Stop the robot and close its connection to the computer

6. Save the robot position data

4. PRE-LAB REPORTS

Exercise 1:

The state machine provided in Figure 11 below makes the robot move to the target position

while avoiding any obstacles in its path. If an obstacle is encountered, the

robot will turn counter-clockwise and proceed to move around the edge of the obstacle. We have

made the assumption that each obstacle encountered is a fixed size and that they are spaced

sufficiently far apart to avoid simultaneiously encountering more than one obstacle. The

defintion for each state and the necessary variables are provided in Table 3 and Table 4. *NOTE:

This allows the robot to travel a shorter distance on the front/back of th obstacle than it does

along the side of the obstacle.

The value of the variable used to count rotations, , will need to be incremented/decremented as

directed after each rotation state is completed.

Figure 11: Obstacle Avoidance State Machine

Table 3: Obstacle Avoidance State Machine Definitions

State Definition End Condition(s)

0 Translate until or object detected | |

1 Rotate robot increment by 1 | | && | | &&

2 Translate | | * | | && | | &&

3 Rotate robot and decrement by 1 | |

4 End

*NOTE: This allows the robot to travel a shorter distance on the

front/back of the obstacle than it does along the side of the obstacle.

Table 4: Obstacle Avoidance State Machine Variables

Variable Definition

X Position Error (in):

Feedback Translation Error (in)*:

Feedback Rotation Error (in)*:

Sonar Distance (in):

*NOTE: The feedback translation ( ) and rotation ( )

are reset to zero after each state is completed (by setting ).

Using this state machine and the map provided below, determine the series of states the robot

will progress through as it moves from its initial position to its final

position . Additionally, note what the value of should be after each state is

completed. Assume the robot starts from , the initial value of the rotation counter is

, and that the sonar sensor angle is fixed at zero. Draw the expected trajectory of the

robot.

Figure 12: Obstacle Map

Exercise 2:

Using proper LabVIEW formula node syntax, write a program to move the robot a specified

distance in inches. The robot should stop if the error between the current and desired position is

less than 0.04 inches or if the sonar detects an object closer than 10 inches. This scenario is

depicted by the following state machine diagram.

Figure 13: Translation State Machine

Use the following controller to set the robot’s velocity.

∫ ̇

Similar to what was done in previous labs, the gains for our controller have been selected by

using the Ziegler-Nichols tuning method. After finding the critical gain, , at which the output

oscillates with a constant amplitude, the critical gain, , and the oscillation period, , are

used with the type of controller desired to select the controller gains:

Exercise 3:

Using proper LabVIEW formula node syntax, write a program to rotate the robot by 90 degrees

counter-clockwise. The robot should stop if the error between the current and desired heading is

less than 0.25 degrees. This scenario is depicted by the following state machine diagram. Use the

same controller gains defined in Exercise 2.

Figure 14: Rotation State Machine

5. LAB PROCEDURE

Exercise 1: Calibration of Position Measurement

The available information from the robot is left encoder, right encoder, left velocity, and right

velocity. Using the encoder data, it is straightforward to calculate the feedback translation and

feedback rotation. This information can be used to approximate the robot position and heading.

However, the robot position or heading may be not accurate because of slippage in the wheels

and due to some constant factors not being correct in the expected environment. Therefore, you

need to find a relationship between the encoder readings and distance and between the encoder

readings and robot heading.

1. Use the keyboard to move the robot a series of specified distances. Stop the robot at each

set distance and record the encoder measurement data in the table provided. NOTE: Do

not reset the encoders after each position.

Table 5: Feedback Translation Calibration

Distance

(in)

Left Encoder

(counts)

Right Encoder

(counts)

Feedback Translation

(counts)

Feedback Rotation

(counts)

24.0

48.0

72.0

2. Use the keyboard to move the robot a series of specified robot headings (i.e. angles). Stop

the robot at each set heading and record the encoder measurement data in the table

provided. NOTE: Do not reset the encoders after each position.

Table 6: Feedback Rotation Calibration

Angle

(deg)

Left Encoder

(counts)

Right Encoder

(counts)

Feedback Translation

(counts)

Feedback Rotation

(counts)

90

180

270

Use the data in these two tables to find the calibration constants between FT and distance in

inches and between FR and angle in degrees.

Exercise 2: Translate Robot

Implement your program from Prelab Exercise 2 to move the robot forward 48 inches. Test the

program both with and without an obstacle placed 36 inches ahead of the robot.

The program automatically stores a graph of the x-axis trajectory, “X Position.bmp”, and places

it in your project directory. Make a copy of this graph for both conditions for your postlab.

Exercise 3: Rotate Robot

Implement your program from Prelab Exercise 3 to rotate the robot by 90 degrees counter-

clockwise.

The program automatically stores a graph of the robot heading, “Th Position.bmp”, and places it

in your project directory. Make a copy of this graph for your postlab.

Exercise 4: Moving to Desired Position with Simple Obstacle Avoidance Algorithm

Using the code developed from the previous two exercises, write a program to move the robot to

the target position while avoiding any obstacles in its path. To accomplish

this, you will need to develop and implement the code corresponding to the state machine

defined in Prelab Exercise 1.

The program automatically stores a graph of the robot trajectory, “Trajectory.bmp”, and places it

in your project directory. Make a copy of this graph for your postlab.

6. POSTLAB AND LAB REPORTS

Exercise 1:

Using the left and right encoder data from Exercise 1, determine the relationship between

encoder counts and robot position.

a) Plot the feedback translation against the actual distances moved by the robot from the

first test (include the origin).

b) Using this graph, find the relationship between the encoder counts and robot distance (in

inches).

c) Plot the feedback rotation against the actual distances moved by the robot (include the

origin) from the second test.

d) Using this graph, find the relationship between the encoder counts and robot heading (in

degrees).

e) When calibrating the robot’s feedback translation, what was the expected feedback

rotation? Similarly, what was the expected feedback translation when testing feedback

rotation? Compare these expectations to your experimental results and explain any

discrepancies.

Exercise 2:

a) Provide a graph of the robot’s x-axis translation both with and without the robot’s path

obstructed by an obstacle.

b) Provide a printout of your working formula node code.

Exercise 3:

a) Provide a graph of the robot’s heading.

b) Provide a printout of your working formula node code.

Exercise 4:

a) Provide a graph of the robot’s trajectory.

b) On the graph, indicate which state corresponds to each segment of the robot’s trajectory

c) Provide a printout of your working formula node code.

APPENDIX

A.1 LABVIEW PROGRAM START-UP AND EXECUTION

1) Download the program archive (“Lab 10.zip”) from the course website

2) Using 7 zip, extract the entire archive to the computer’s hard drive

a) Double-click the zipped archive

b) Press 7 to open the file in 7-zip

c) Press Extract and select the folder D:\USERS\

d) Click OK and close 7-zip

3) Launch LabVIEW 2012

a) Start → All Programs → Local → ni →

National_Instruments_LabVIEW_2012_SP1_32-bit_

4) Open the top-level project file

a) Select Open Existing

b) Locate and select the file “Lab 10.lvproj” in the Lab 10 folder you just extracted

The top-level project file contains a directory of all the hardware and programs associated

with a given project. A view of the project directory we are working with for this lab can be

seen in Figure A1 below.

Figure A1: LabVIEW Project Structure

As shown, the project can be broken up into two main sections: (1) code that is compiled

and executed on the computer and (2) code that is compiled and executed directly on the

robot. The FPGA code on the robot has been pre-compiled and will not need to be

modified. Accordingly, we will only be working with programs compiled and executed

on the computer.

5) Open the main program to control the robot

a) Choose “Robot - Formula Node Control” to control the robot using logic defined in a

formula node or “Robot - Manual Control” to control the robot manually.

i) Exercise 1: Robot - Manual Control

ii) Exercise 2, 3, and 4: Robot - Formula Node Control

b) Run the program by clicking the Run button.

The program top level commands in LabVIEW are provided in the top left corner of

every program window. These can be seen in Figure A2 below.

Figure A2: LabVIEW Program Execution Controls

In addition to starting the program, these buttons can be used to pause or abort a program

during operation. However, when possible use the specific control buttons available

within the LabVIEW program provided. These internally programmed buttons allow you

to properly close connections to the robot hardware.

6) Connect the robot

a) Using a LAN cable provided, connect the robot to the nearest wall Ethernet port.

b) Turn the robot MASTER and MOTORS switches from OFF to ON (see Figure A3)

Figure A3: Robot Controls, Connection, and Identification

With the MASTER switch off, the computer cannot connect to the robot. This switch

removes power from the entire robot. The MOTORS switch only removes power from

the robot’s motors. If due to an error in programming the robot moves uncontrollable or

is about to crash, the MOTORS switch can be turned off as a failsafe to stop motion.

c) Use the drop down menu provided to select the robot number

d) To confirm you selected the correct robot, type the 5-digit serial number found on the

bottom of your robot and hit <ENTER> on your keyboard

NOTE: The robot takes a minute to establish a proper connection through the buildings

network. If you try to connect through LabVIEW before this start-up time, your

connection attempt may fail.

7) Control the robot

a) If running the program “Robot - Manual Control”, use the keys defined in Figure A4

below to command the desired robot motion.

Figure A4: Manual Robot Commands

b) If running the program “Robot - Formula Node Control”, all control commands must be

programmed in the formula node provided in the LabVIEW block diagram (Refer to

Appendix 10.A.2 for properly defining a LabVIEW formula node)

c) In both MAIN programs, the robot position estimate can be reset to zero at any time by

pressing the RESET button.

d) Stop the robot and complete the program by pressing the STOP ROBOT button at any

time.

A.2 FORMULA NODE

The formula node is used to evaluate mathematical formulas and expressions similar to C on the

block diagram. The following built-in functions are allowed in the formula node: abs, acos,

acosh, asin, asinh, atan, atan2, atanh, ceil, cos, cosh, cot, csc, exp, expm1, floor, getexp, getman,

int, intrz, ln, lnp1, log, log2, max, min, mod, pow, rand, rem, sec, sign, sin, sinc, sinh,

sizeOfDim, sqrt, tan, tanh.

Figure A5: Formula Node

It is very important to realize that every variable declared inside the formula node will be deleted

after completing each iteration of the tasks inside the formula node.

Therefore, if you want a stored variable, you need to send the stored variable out of the while

loop and send the stored variable back to the while loop as a new input.

The equivalent of a state machine is accomplished by using “switch statements” within the

formula node.

switch(state){

case 0:

// Insert logic for state 0

break;

case 1:

// Insert logic for state 1

break;

case n:

// Insert logic for state n

break;

}

Here, the variable “state” is used to select the desired case to use for this iteration through the

switch statement. Note that each defined case must be terminated with a break statement.

A.3 BASIC LABVIEW PROGRAMMING ELEMENTS

Table A1: Color Code Definitions

Color Line Control Indicator Data Type

Single, Double, or Extended floating-point

Fixed Point

8, 16, 32, 64 bit signed integer

8, 16, 32, 64 bit unsigned integer

Boolean

String

Reference

Error

Table A2: While Loop Definitions

Element Definition

While-loop

Loop period

Loop iteration

Conditional terminal: Stop loop if True

Shift Register: Feedback variables for next loop iteration

Table A3: “Sub-VI” Block Diagrams

“Sub-Vi” Block Purpose Inputs Outputs

Select

Robot

Select the desired robot and

enter its unique 5 digit

confirmation code (serial

number)

Robot Number

Confirmation Code

Robot Number

Status

Robot Selected?

Robot Selection Confirmed?

Initialize

Robot

Establish a connection to the

robot with LabVIEW

Robot Number FPGA VI Reference

Error

Initial Left Encoder (counts)

Initial Right Encoder (counts)

Stop

Robot

Stop the robot and close its

connection with LabVIEW

FPGA VI Reference

Error

Read

Encoders

Read the encoder data from

the robot

FPGA VI Reference

Error

Initial Left Encoder (counts)

Initial Right Encoder (counts)

Reset Encoders

FPGA VI Reference

Error

Initial Left Encoder (counts)

Initial Right Encoder (counts)

Left Encoder (counts)

Right Encoder (counts)

Feedback Translation (counts)

Feedback Rotation (counts)

Left CCW Velocity (rad/s)

Right CCW Velocity (rad/s)

Read

Sonar

Read the sonar data from the

robot

FPGA VI Reference

Error

FPGA VI Reference

Error

Sonar Distance (in)

Filter

Sonar

Filter the sonar data with a

second order low pass filter

Sonar Distance (in)

Sonar Distance Filtered (in)

Filter

Velocity

Filter the robot velocity

estimate with a second order

low pass filter

Left CCW Velocity (rad/s)

Right CCW Velocity (rad/s)

Command

Rotational Velocity (rad/s)

Translational Velocity (in/s)

Robot

Motor

Command

Generate commands to

control the robot motors

FPGA VI Reference

Error

Command

Velocity (rad/s)

FPGA VI Reference

Error

Sonar

Motor

Command

Generate a command to

control the sonar motor

FPGA VI Reference

Error

Sonar Angle (deg)

FPGA VI Reference

Error

Update

Robot

Position

Estimate the robot’s current

position based on the current

encoder data and previous

position estimate

Feedback Translation (counts)

Feedback Rotation (counts)

Initial X Position (in)

Initial Y Position (in)

Initial Th Position (deg)

Reset Encoders

Reset Position

Counts per Inch

Counts per Degree

X Position (in)

Y Position (in)

Th Position (deg)

Create

Map

Generate a known map of the

robot environment

Map

Plot

Robot

Position

Create a plot of the robot’s

position data

X Position (in)

Y Position (in)

Th Position (deg)

Sonar Distance (in)

Sonar Angle (deg)

Map

Iteration

Maximum Plot Size

Reset Position

X Position (in) Plot

Y Position (in) Plot

Th Position (deg) Plot

Trajectory (in,in,deg) Plot

Plot

Sonar

Create a plot of the original

and filtered sonar data

Sonar Distance (in)

Sonar Distance Filtered (in)

Iteration

Maximum Plot Size

Reset Position

Sonar Distance (in) Plot

Sonar Distance Filtered (in) Plot