Java程序辅导

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  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