RFID Tag Seeking Robot

A robot that allows a user to assign unique names to red-colored RFID tags and then correctly seek out a requested tag.

ECE 5725
May 18, 2018


Demonstration Video


Introduction

We built an autonomous robot that searches for and then navigates to red RFID tagged objects. Our robot is composed of a Raspberry Pi, a camera, an RFID scanner, and a speaker mounted on a frame with two servos for wheels. The robot uses computer vision and scans for the color red. When a red color is identified, the robot navigates to the color by keeping the center of the detected color in the viewing area. While navigating, the robot is continuously scanning for an RFID signal. When the robot is close enough to the object to detect the RFID tag, the robot compares to the Unique Identification Number (UID) of the detected tag to the target UID. If the UIDs match, the robot stops and plays a celebratory song to alert the user of the tag's location. If the UIDs do not match, the robot continues to search until it finds the target tag.


RFID Seeking Robot Photo

Project Objective:

  • Autonomously seek out a requested RFID tag and alert the user once it has done so (by playing an alarm).
  • Visually identify and approach red-colored RFID tags to allow for tag scanning.
  • Ability to scan RFID tags and add/compare their unique identification information (UID) to an internal database.
  • Allow the user to assign their own unique object-identifying names to each tag and retain a database of these tag-name assignments.
  • Have a fully-functional user interface to allow the embedded system to operate completely untethered.

Design

Our robot is made up of a RaspberryPi Model 3B, a PiCamera V2, an Adafruit PN532 RFID scanner, and a mini speaker strapped to a plastic frame driven by two parallax servo wheels. The RaspberryPi interfaces with the servos and RFID scanner via GPIO output pins wired up to a miniature breadboard. The PiCamera interfaces directly with the RaspberryPi and the mini speaker simply plugs into the Pi's AUX audio port. This RFID Seeking Robot is a fully embedded, untethered system that will begin operation immediately on startup.

On startup, the RaspberryPi will run the bash script, RFID_Track, that we placed in the /etc/rc.local file of our Pi (See Code Appendix). Because rc.local is read on startup but before the system has fully initialized, RFID_Track starts by waiting 5 seconds via the StartUpSleep.py script (See Code Appendix) to ensure the system has finished initializing, then runs our primary script, RFID_Detect_Main.py (See Code Appendix). RFID_Detect_Main.py is responsible for interfacing with the PiCamera and wheel servos to visually identify the red RFID tags and navigate over to them during searches. RFID_Detect_Main.py also interfaces with the PiTTFT touchscreen to provide a fully functional and interactive user interface to handle RFID tag management. The C executable RFID_simple_read_v2 (See Code Appendix) is responsible for interfacing with the PN532 RFID/NFC scanner to detect and read RFID tag data, but it is RFID_Detect_Main.py that determines when RFID_simple_read_v2 is executed and extracts tag information output from RFID_simple_read_v2 as a string.

Hardware Wiring

The Adafruit PN532 RFID/NFC scanner and two parallax servos were connected to the RaspberryPi's GPIO output pins as shown below in Figure 1.

Wiring Diagram

Figure 1: Hardware Wiring Diagram

We wired up the scanner to our Raspberry Pi as shown here in this PN532 Installation Guide. Note that the SEL0 and SEL1 jumpers on the board are set to OFF. The parallax servos were wired as described in the Parallax Standard Servo Datasheet. The PiCamera, being designed specifically to interface with the RaspberryPi, plugged directly into the camera port on the Raspberry Pi.

User Interface

Our Raspberry Pi uses PyGame to feature a fully functional user menu interface that the user can interact with and navigate via the PiTFT touchscreen. Through this interface, the user can add new tags to the internal database and assign a 6-character unique name to any tag. The interface additionally allows for the renaming and deleting of stored tags, and will detect if a tag a user is trying to add already exists within the system. The interface is capable of storing and listing out a total of 24 tags, listed across 3 pages with 8 tags per page, though the system could easily be scaled up to handle more pages with a few additional lines of code. The "List" menu will automatically update whenever changes are made to the system's internal database of RFID tags, and stored tags can be individually selected and manipulated from this list. To search for a specific tag, for instance, the user need only select "Search" from the main menu, navigate through the list, pick the tag they wish to search for, and hit confirm to make the robot seek out that tag. A full menu flow-diagram can be seen below:

Final Menu Design Diagram

Figure 2: Final menu design diagram

OpenCV Visual Tracking

To navigate, the robot uses the Raspberry Pi Camera Module v2 and computer vision. To set up the camera, the PiCamera was plugged into the camera port on the Raspberry Pi. To allow for real-time computer vision, the OpenCV (v2.4.9) library was downloaded and installed using the "sudo apt-get install libopencv-dev python-opencv" command on our 4.4.50 Preempt RT kernel:

$ sudo apt-get install libopencv-dev python-opencv
To allow for detection of the red RFID tags, the PiCamera library was also installed. The Image Processing in OpenCV python tutorial was consulted to make the image_process() function in our RFID_Detect_Main.py script. This image_process() function uses the PiCamera library to convert the Pi Camera's captured Red, Green, and Blue (RGB) frame into matrix form. This RGB input was then converted into a Hue, Saturation, Value (HSV) color model using the "cv2.cvtColor(image, cv2.COLOR_BGR2HSV)" command. The HSV color space is best for color-based image segmentation as the R, G, and B components responsible for an object's color depend on the amount of light incident on the object. Thus, there is a dependence of each pixel on the surrounding pixels that makes image segmentation difficult using the RGB color space. The HSV color space, however, allows for the capture of the hue of the object without a dependence on lighting conditions. Once the color space has been converted, using an upper and lower bound of the target color, the raw image colors are thresholded into a binary matrix of the pixels that were detected to be the target color. To allow for better contour detection by removing the noise, the morphological operations of erode and dilate were applied to the binary outputs. As sometimes the returned contours of the target object were not the entire object but rather subsets of the object, only the largest contour area was used for navigation.

Robot Navigation

Robot navigation while searching is handled by the NavMain() function of our RFID_Detect_Main.py script. When navigating, the robot begins by pivoting right to search for the target color. Once the target color has been detected, the robot takes the largest contour detected and locates its center of "mass". The getCOM() function in the navigation part of code uses the moments function of OpenCV to obtain the moments of the contour and then determines the central moments in the x-y plane. Once the COM is determined, the robot navigates by trying to keep the COM of the target in the center of the camera's view. If the COM of the detected object is in the left quarter of the camera view, the robot pivots left to center the object. If the detected object is in the right quarter of the camera view, the robot pivots right to center the object in the camera view. If the detected object is in the middle half of the camera view, the robot moves straight. In the case where an object is detected in the previous frame, but not in the current frame, the robot corrects its course in one of two ways. If the robot is far away, it corrects using the correct() function by pivoting towards the side where the object was last directed and then moving forward. If the robot is close to the target object, it corrects using the scan() function to pivot in place. The need to move forward when correcting for far away objects was determined experimentally to speed up the search process as the scan() function tended to be really sensitive at far distances as the object took up a small part of the camera view and pivots could cause the COM to jump from the right quarter of the screen to the left quarter of the screen or vice versa. These jumps would cause the robot to do the opposite correction pivot than the previous one without moving any closer to the object. Thus, the robot could get stuck in the scan function for a while at far distances from the object. At close distances, as the RFID scanner had a limited range and needed a particular alignment to register the tag, the scan() function was written to optimize the scanning arc covered by the robot. In the extreme quarters of the camera view, the scan() function operates the same way as the correct() function, however, over the middle half of the viewing area the robot pivots in a way to have the COM of the detected object move towards the outer quarter of the viewing area. Once the target object had been detected by the RFID scanner, if the object was not the desired object, the robot would back up and pivot right to start the scanning process again.

RFID Scanning

Our robot features an Adafruit PN532 RFID/NFC scanner attached to its front directly below the PiCamera. We wired up the scanner to our Raspberry Pi as shown here in this PN532 Installation Guide (See Figure 1), then downloaded and installed the libnfc library. Using the libnfc library, we coded a C executable (compiled from RFID_simple_read_v2.c) that will constantly poll for nearby RFID tags. RFID_simple_read_v2 will not stop running until it has detected a tag, the process has been manually canceled, or an error in connecting with the scanner occurs. This process will return the ATQA, UID, and SAK of the RFID tag if a tag is detected. It will only print an ATQA value if it detects a tag, so we used this fact to extract the UID printed by RFID_simple_read_v2 as a string to be used by our RFID_Detect_Main.py code.

Whenever our RFID_Detect_Main.py code needs to scan for a tag, it uses the subprocess.Popen command to run RFID_simple_read_v2 in the background and pipe its output to a .txt file (NFC.txt) once it finishes. The subprocess.poll() command is used in a while loop to determine whether or not RFID_simple_read_v2 is still running. If RFID_simple_read_v2 has stopped running, either a tag has been found or the Pi cannot connect to the scanner (which triggers an error). The ScanRFID() function in RFID_Detect_Main.py looks for the presence of "ATQA" in the output piped to NFC.txt, then from this determines whether or not a tag was actually detected or if a connection error occurred. If a connection error occurred (signified by a lack of "ATQA" in the output), RFID_Detect_Main.py runs RFID_simple_read_v2 again until it successfully connects to the scanner. If "ATQA" is present in the output and thus a tag was detected, the ScanRFID() function extracts the UID of the detected tag as a string to be used later by the RFID_Detect_Main.py code.

The Object_Def dictionary stores the names and UIDs of all stored tags as a key:value pair (Name:UID). Once the UID of a scanned tag is extracted as a string by ScanRFID(), it can be added to Object_Def and/or compared to other stored UIDs during a search to determine if the detected tag is the one the robot was asked to find or if it should keep searching. A UID is typically unique to each RFID tag. In the rare case where two tags share the same UID, the script alerts the user that the tag's UID is already in the system.

Between sessions, the contents of Object_Def is stored in a .csv file (Object_Def.csv), allowing the system to retain RFID tag Name:UID assignments even after shutdown. Object_Def is populated by Object_Def.csv on startup and Object_Def.csv is updated any time the Object_Def dictionary is updated or the user quits RFID_Detect_Main.py.

Robot Frame & Movement Functions

Our robot moves thanks to two Parallax Continous Rotation Rotation Servos (#900-00008) attached to the rear of a plastic frame (provided in the lab portion of this course) with a rolling ball-bearing at the front to provide balance. The parallax servos were wired up as described in the Parallax Standard Servo Datasheet (See Figure 1). The servos were controlled using Pulse width Modulation (PWM). Four movement functions were written: pivot_left(), pivot_right(), backward(), and straight(). These functions are located in our RFID_Detect_Main.py script (See Code Appendix). All movements have servos spinning at half speed. The frequencies and duty cycles corresponding to each movement were determined in a lab for this class. It is worth noting that the pivot_left() function works by having the left wheel turn backwards and the right wheel turn forwards. Similarly, the pivot_right() function works by having the right wheel turn backwards and the left wheel turn forwards.


Drawings

Below you will find various conceptual drawings and scribbles made throughout the design and build of our robot. The drawings are presented here more to provide a window into our design thought process rather than to provide detailed schematics of our system. Many of the designs/concepts shown in the drawings below have changed since our final demonstration, so take care when referencing them if you are trying to recreate our project.

Early menu concept

Early menu design concept diagram

Camera View Debugging Drawing

Debugging of Coordinates of Camera View

Early Scanning Concept Drawing for Test Arena

Early Scanning Concept Drawing for Test Arena


*The above drawings are original works by Dante Del Terzo & Alec Cornwell


Testing

One of the first issues we ran into occurred while we were initially testing our RFID scanner. The PN532 RFID scanner has a tendency to "sleep" when not in use, sometimes resulting in a connection error the first time you attempt to connect to it after a long period of inactivity. We are still unsure of the cause of this "sleep" (whether it is a hardware error or a feature), but we noticed that the scanner would work fine on subsequent scan calls after the initial error "woke it up". So to ensure that our scanner does not time out while the user is having it search for a tag, we realized a quick fix was to have our script automatically try to run the RFID scanning code again should the first attempt result in a connection error output (See Design-RFID Scanning to learn more about how we do this). This fix allowed us to have our code reliably contact our RFID scanner every time with no noticeable degradation to system performance nor reaction time.

Another issue we were able to fix through testing is an issue we have had with our PiTFT since its initial installation. We are unsure of the cause, but our PiTFT does not initialize touchscreen functionality properly on startup, resulting in a "This is not a touchscreen I recognize" error message and an inability to use the touchscreen. Prior to our fix, the only known way to fix this was to uninstall and then reinstall the touchscreen driver every time the Pi is turned on using the 'sudo rmmod stmpe_ts' then 'sudo modprobe stmpe_ts' commands:

$ 'sudo rmmod stmpe_ts' 
$ 'sudo modprobe stmpe_ts'
Previous attempts to automatically enter in these commands into the shell on startup via a bash script had mixed results, and did not reliably ensure touchscreen functionality on startup. The breakthrough occurred when we tried using subprocess.call commands within the initialization section of our python code itself to uninstall and reinstall the driver, like so:
# Imports
import subprocess
import time

...

# Initialize TFT Touchscreen

#Remove 'stmpe_ts' touchscreen driver
subprocess.call('sudo rmmod stmpe_ts', shell=True)
#Sleep to ensure driver is removed before proceeding
time.sleep(0.2)
#Reinstall 'stmpe_ts' touchscreen driver
subprocess.call('sudo modprobe stmpe_ts', shell=True)
#Sleep to ensure driver is readded before proceeding
time.sleep(0.2)

...
Uninstalling and reinstalling the touchscreen driver using these subprocess commands at the start of our python code has thus far reliably ensured touchscreen functionality on every iteration of our code. More testing needs to be done to determine whether this fix will work for everyone with the same "is not a touchscreen I recognize" error, or whether the time.sleep commands are actually necessary, but this method has worked far more reliably than any other autonomous fix our class has found thus far.

We ran into some initial trouble using the Pi Camera for image processing. We found that a lot of the documentation is for USB cameras which have different OpenCV functions that the Pi Camera. In particular, this OpenCV-PiCamera tutorial was used to set up the Pi Camera. To set up image processing the Image Processing in OpenCV python tutorial was consulted for changing colorspaces, image thresholding, morphological transformations, and creating contours. However, this tutorial was for a USB camera so the Robotic Candy Sorter Project from a previous Cornell ECE 5725 course was also consulted. Lastly, this OpenCV Moments documentation was consulted to find the COM of a detected object.

Our setup of the camera on the robot led to some issues with designing and testing the code. As the Pi Camera is tethered to the Pi, there was only one way to feasibly mount the camera on the front of the robot. In the feasible configuration, the camera was actually upside down. This flip made it necessary to pay close attention to the coordinate system of the camera view as the code relies on the x-coordinate of the COM of the red object for navigation. The figure below illustrates the coordinate system of the camera's viewing area.

Coordinates of the camera view.

Figure 3: Camera's View Coordinates

Another vision issue we ran into was that the ability of the robot to detect the red object was very dependent on the lighting. To fix this problem, the saturation values for the upper and lower HSV bounds for red were widened. With this change, the robot can consistently identify the red tags in both dim and bright environments.

For a time, we also ran into an issue where the PiCamera would fail and throw an error, claiming to be "out of resources", if the PiCamera was ever initialized twice in a single session. Once initialized, the PiCamera would remain so even after quitting the script, causing errors on subsequent runs of the script and preventing the script from running until the Pi was rebooted. To ensure that the camera is not initialized twice in the same run of the RFID_Detect_Main.py script, an "if" statement was added to the NavMain() function to only initialize the camera once per run of the script. To ensure that the camera does not remain initialized after the script is exited, we had to add a camera.close() cleanup command to the end of our script. With these changes, we no longer ran into any issues with our PiCamera running out of resources and forcing our script to exit prematurely.


Result

We were successfully able to create an autonomous RFID seeking robot. The user could enter a desired object for the robot to search for. When the object was found an alarm would alert the user of the location. The robot used computer vision to identify the color red and navigated to red colors by keeping the center of the detected red in the center of the camera view. When arriving at the red object, the robot was able to scan RFID tags to obtain their unique identification information (UID) and compare them to the to the UID of the target object. Additionally, the interface offered the ability to add/overwrite UIDs to an internal database with unique names. This fully functioning user interface allowed the embedded system to operate completely untethered.

We are extremely happy with the end result of our project. For future work we would like to find a way to increase the range of the scanner so that tags can be placed anywhere on an object. Currently the scanner has an approximate two-inch range only in a direction perpendicular to the scanner. This limited range led to the tags having to be placed at the same height on every object. Furthermore, the fact that the scanner only saw straight in front of it led to some problems if the tagged object was curved and the scanner was not looking straight at the tag. Other than this, our system worked very well. Our only other wish was that there was a greater part of the semester dedicated to projects so that we could have had time to get to our stretch goals of incorporating collision avoidance and object retrieval in the project.


Work Distribution

Group Photo

Project group picture

Dante Photo

Dante Del Terzo

dnd37@cornell.edu

Designed & coded the user interface as well as the RFID reading/writing algorithms. Ensured proper integration of user interface, RFID scanning, and robot navigation codes into a single primary script.

Alec Photo

Alec Cornwell

amc578@cornell.edu

Assembled hardware and installed the appropriate libraries including OpenCV and libnfc. Designed & coded the visual tracking and robot movement algorithms.


Parts List

Total: $169.38

Total of Non-Lab-Provided Parts: $99.43


References

PyGame Documentation
PiCamera Document
Using OpenCV with a Pi Camera Tutorial
OpenCV 2.4.9 Documentation
OpenCV Moments
Image Processing in OpenCV
Robotic Candy Sorter (referenced for image processing)
PN532 Installation Guide
Libnfc Documentation
Parallax Standard Servo Datasheet
R-Pi GPIO Document
"Playing Sound On Raspberry Pi" (source of example.mp3)
Hilite.me for HTML Code Conversion


Code Appendix

RFID_simple_read_v2
RFID_Detect_Main.py
StartUpSleep.py
RFID_Track
Edited rc.local

RFID_simple_read_v2.c

// dnd37_amc578
// May 13, 2018
// RFID_simple_read_v2.c
// Simple RFID reading code.

// Based on code provided at: www.libnfc.org/api/examples_page.html

//  To compile:
// $ gcc -o RFID_simple_read_v2 RFID_simple_read_v2.c -lnfc

#include <stdlib.h>
#include <nfc/nfc.h>

static void
print_hex(const uint8_t *pbtData, const size_t szBytes)
{
    size_t  szPos;

    for (szPos=0; szPos < szBytes; szPos++) {
        printf("%02x  ", pbtData[szPos]);
    }
    printf("\n");
}

int
main(int argc, const char *argv[])
{
    nfc_device *pnd;
    nfc_target nt;

    // Allocate only a pointer to nfc_context
    nfc_context *context;

    // Initialize libnfc and set the nfc_context
    nfc_init(&context);
    if (context == NULL) {
        printf("Unable to init libnfc (malloc)\n");
        exit(EXIT_FAILURE);
    }

    // Display libnfc version (uncomment to use)
    //const char *acLibnfcVersion = nfc_version();
    //(void)argc;
    //printf("%s uses libnfc %s\n", argv[0], acLibnfcVersion);

    // Open, using the first available nfc device which can be in order of selection
    //  - default device specified using environment variable or
    //  - first specified device in libnfc.conf (/etc/nfc) or
    //  - first specified device in device-configuration directory (/etc/nfc/devices.d) or
    //  - first auto-detected (if feature not disabled in libnfc.conf) device
    pnd = nfc_open(context, NULL);

    if (pnd == NULL) {
        printf("ERROR: %s\n", "Unable to open NFC device.");
        exit(EXIT_FAILURE);
    }

    // Print NRC reader name (uncomment to use)
    //printf("NFC reader: %s opened\n", nfc_device_get_name(pnd));

    // Poll for a ISO14443A (MIFARE) tag
    const nfc_modulation nmMifare = {
        .nmt = NMT_ISO14443A,
        .nbr = NBR_106,
    };
    if (nfc_initiator_select_passive_target(pnd, nmMifare, NULL, 0, &nt) > 0) {
        printf("The following (NFC) ISO14443A tag was found:\n");
        printf("    ATQA (SENS_RES): ");
        print_hex(nt.nti.nai.abtAtqa, 2);
        printf("       UID (NFCID%c): ", (nt.nti.nai.abtUid[0] == 0x08 ? '3' : '1'));
        print_hex(nt.nti.nai.abtUid, nt.nti.nai.szUidLen);
        printf("      SAK (SEL_RES): ");
        print_hex(&nt.nti.nai.btSak, 1);
        if (nt.nti.nai.szAtsLen) {
            printf("          ATS (ATK): ");
            print_hex(nt.nti.nai.abtAts, nt.nti.nai.szAtsLen);
        }
    }
    // Close NFC device
    nfc_close(pnd);
    // Release the context
    nfc_exit(context);
    exit(EXIT_SUCCESS);
}


RFID_Detect_Main.py

# dnd37_amc578
# May 16th, 2018
# RFID_Detect_Main.py
#
# Main code for RFID seeking robot. Robot searches for a red target. If target is found, robot 
# navigates to the target and reads the RFID tag associated with the robot. If tag is the one it
# is searching for, the robot stops and plays a found song. If found tag is not the desired tag
# the robot begins searching again.

# Imports
import RPi.GPIO as GPIO
import os
import os.path
import sys
import csv
import math
import subprocess
from picamera.array import PiRGBArray
from picamera import PiCamera
import cv2
import numpy as np

# TFT Printing Initialization
import time
import pygame
from pygame.locals import * 			# Touch defines

GPIO.setmode(GPIO.BCM) 					# set from broadcom numbering, not board
GPIO.setup(12, GPIO.OUT) 				# Servo 1 (RIGHT)
GPIO.setup(16, GPIO.OUT) 				# Servo 2 (LEFT)

# Setup a GPIO for input buttons
GPIO.setup(17,GPIO.IN,pull_up_down=GPIO.PUD_UP)

# Initialize TFT Touchscreen
subprocess.call('sudo rmmod stmpe_ts', shell=True)
time.sleep(0.2) 						#Sleep to ensure driver is removed before proceeding
subprocess.call('sudo modprobe stmpe_ts', shell=True)
time.sleep(0.2) 						#Sleep to ensure driver is readded before proceeding

# Setup TFT Display for Quit button on touchscreen
os.putenv('SDL_VIDEODRIVER','fbcon') 	#display on TFT
os.putenv('SDL_FBDEV','/dev/fb1')
os.putenv('SDL_MOUSEDRV','TSLIB') 		#Track mouse clicks on TFT
os.putenv('SDL_MOUSEDEV','/dev/input/touchscreen')

# PWM Servo initialization
global p1
global p2								
p1 = GPIO.PWM(12, 46.51163)				# Right Wheel 
p2 = GPIO.PWM(16, 46.51163)				# Left Wheel
#p1.start(0)
#p2.start(0)

# Define Global Variables
global Scanning
global ScanCancel
global Search_UID
global SongProcess
global MENU
global LastMENU
global RFID_found
global RFID_UID
global Q

global colorLB							# lower bound for target color
global colorUB							# upper bound for target color
global camera
global lastX							# last x of the center of mass of target color
global lastSize							# last size of the target color

global CamInit
CamInit = False


def straight():
	# function to have the robot move straight

	print("S")
	global p1
	global p2
	
	# Frequencies and Duty Cycles that correspond to robot moving straight at half speed
	p1.ChangeFrequency(1/0.0214)
	p1.ChangeDutyCycle(100*(0.0014/0.0214))
	p2.ChangeFrequency(1/0.0216)
	p2.ChangeDutyCycle(100*(0.0016/0.0216))

	time.sleep(.1)						# sleep 0.1s

	# stop wheels after going straight
	p1.ChangeDutyCycle(0)
	p2.ChangeDutyCycle(0)

def backward():
	# function to have robot backup straight

	print("B")
	global p1
	global p2
	
	# Frequencies and Duty Cycles corresponding to robot moving backwards in straight line
	# at half speed
	p2.ChangeFrequency(1/0.0214)
	p2.ChangeDutyCycle(100*(0.0014/0.0214))
	p1.ChangeFrequency(1/0.0216)
	p1.ChangeDutyCycle(100*(0.0016/0.0216))

	time.sleep(.1)						# sleep 0.1s

	# stop wheels after backing up
	p1.ChangeDutyCycle(0)
	p2.ChangeDutyCycle(0)
	

def pivot_left():
	# function to have the robot pivot left
	
	print("L")
	global p1
	global p2

	# Frequencies and Duty Cycles to have right wheel spin forward and left wheel spin backward
	p1.ChangeFrequency(1/0.0214)
	p1.ChangeDutyCycle(100*(0.0014/0.0214))
	p2.ChangeFrequency(1/0.0214)
	p2.ChangeDutyCycle(100*(0.0014/0.0214))

	time.sleep(.1)						# sleep 0.1s

	# stop wheels after pivoting
	p1.ChangeDutyCycle(0)
	p2.ChangeDutyCycle(0)

def pivot_right():
	# function to have the robot pivot right

	print("R")
	global p1
	global p2
	
	# Frequencies and Duty Cycles to have left wheel spin forward and right wheel spin backward
	p1.ChangeFrequency(1/0.0216)
	p1.ChangeDutyCycle(100*(0.0016/0.0216))
	p2.ChangeFrequency(1/0.0216)
	p2.ChangeDutyCycle(100*(0.0016/0.0216))

	time.sleep(.1)						# sleep 0.1s

	# stop wheels after pivoting
	p1.ChangeDutyCycle(0)
	p2.ChangeDutyCycle(0)

def getCOM(contours):
	# function to get center of "mass" of the target
	# takes contours as its input which are the contours of the target

	cnt = contours[0]
	M = cv2.moments(cnt)				# get moments
	cx = int(M['m10']/M['m00'])			# x center of target
	cy = int(M['m01']/M['m00'])			# y center of target
	return (cx, cy)

def scan():
	# scanning function for in place adjustments for close targets

	global lastX
	print("lastX: ", lastX)

	# did not detect object in last frame so pivot right to look for target
	if lastX == 0:
		pivot_right()

	# detected target on far right, turn right
	elif lastX <= 150:
		pivot_right()
	
	# detected target near right, turn left
	elif lastX <= 300:
		pivot_left()

	# detcted target on near left, turn right
	elif lastX <= 450:
		pivot_right()

	# detected target on far left, turn left
	else:	
		pivot_left()

	lastX = 0							# reset last X

def correct():
	# if small target correct and move forward

	global lastX
	print("correct")

	# no target detected, pivot rigth
	if lastX == 0:
		pivot_right()

	# target on the left, pivot left and move forward
	elif lastX <= 300:
		pivot_left()
		straight()
	
	# target on the right, pivot right and move forward
	else:
		pivot_right()
		straight()

def nav():
	# function to navigate to target and scan RFID tag

	print("nav")
	global lastX
	global lastSize
	global Scanning
	global SongProcess
	global ScanCancel
	global RFID_found
	global MENU
	global LastMENU
	global Q

	# while scanning for RFID, navigate
	while Scanning==True and Q==False:
		lastX = 0						# reset lastX
		lastSize = 0					# reset last Size
		
		# get the target's x center, y center, and size
		((cx, cy), area) = image_process()

		#hasArrived = ((cx, cy) == (500, 600))
		
		Moving = True					# set moving to true to start scanning and nav

		# Start scanning for RFIDs while moving
		cmd = 'sudo /home/pi/RFID_Project/RFID_simple_read_v2 > /home/pi/RFID_Project/NFC.txt'
		ScanProcess = subprocess.Popen(cmd, shell=True)
		while Moving==True and Q==False:
			print("COM: ", (cx, cy), "lastX: ", lastX, "lastSize: ", lastSize)
			
			# Target not  detected
			if (cx, cy) == (0,0):
				
				# target was detected in previous frame, but was small:
				#  try correcting to bring back into frame
				if lastSize < 3000:
					correct()
				
				# target was detected in last frame and was large: scan in place for it
				else:
					scan()
			
			# target on right side of screen, pivot right to bring back into frame
			elif cx < 150:
				pivot_right()

			# target on left part of screen, pivot left to bring back into frame
			elif cx > 450:
				pivot_left()

			# target relatively centered on screen, go straight
			else:
				straight()

			lastX = cx					# update LAst X center	
			lastSize = area				# update last area size
			
			# get new info of target location and size
			((cx, cy), area) = image_process()
			
			ScanProcess.poll() #check if still scanning. Returns NONE if still running ScanProcess

			if ScanProcess.poll() == None: #If scanning process still running
				for event in pygame.event.get(): #For cancel button
					if (event.type is MOUSEBUTTONDOWN): #If mouse button clicked down
						pos = pygame.mouse.get_pos()
					elif (event.type is MOUSEBUTTONUP): #If mouse button is released
						pos = pygame.mouse.get_pos()
						x,y = pos
						if (x>0 and y>0):
							ScanProcess.kill() #Stop scanning
							Scanning=False
							Moving = False
							LastMENU = MENU #Save last menu for 'try again' option
							ScanCancel = 'RFID search stopped by user.' #Update reason for stop
							MENU = 'ScanningStopped'
				print("Still scanning")
			else:
				print("No longer scanning")
				Scanning=False
				Moving = False
				ScanRFID()
				if RFID_found == True:
					if RFID_UID == Search_UID:
						print('OBJECT FOUND! PLAY SOUND!')
						MENU = 'ObjectFound'

						# PLAY SOUND
						# Use omxplayer to loop the mp3 and play sound out of the local (headphone
						#  jack) output
						SongCMD = 'omxplayer --loop --no-osd -o local /home/pi/RFID_Project/example.mp3'
						SongProcess = subprocess.Popen(SongCMD, shell=True, stdin=subprocess.PIPE)
					
					# found wrong RFID, keep looking
					else:
						RFID_found = False
						Scanning = True	
						
						# backup and pivot to keep looking
						for i in range(5):
							backward()
						for i in range(4):
							pivot_right()

				elif RFID_found == False: #If RFID scanner times out, restart it
					Scanning = True

	print("ARRIVED!!")


def init_camera():
	print("init_camera")
	# initialize the camera and grab a reference to the raw camera capture
	global camera, rawCapture
	camera = PiCamera()
	camera.resolution = (640, 480)
	camera.framerate = 15
	rawCapture = PiRGBArray(camera)

	# allow the camera to warmup
	time.sleep(0.5)
 

def image_process():
	# capture frames from the camera
	
	COM = (0,0) 							# no image found initially
	
	# color range for red target objects
	colorLB = (149, 116, 83)
	colorUB = (189, 235, 216)
	for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
		# grab the raw NumPy array representing the image, then initialize the timestamp
		# and occupied/unoccupied text
		image = frame.array
		imageRaw= image.copy()
		hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
		mask = cv2.inRange(hsv, colorLB, colorUB)
		mask = cv2.erode(mask, None, iterations = 2)
		mask = cv2.dilate(mask, None, iterations = 2)
 		
		# find contours of target
		contours = cv2.findContours(mask.copy(), 
			cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
		largestC = 0
		
		for c in contours:
			# if the contour is too small, ignore it
			cArea = cv2.contourArea(c)
			if ((cArea > 100) and (cArea > largestC)):	
				# compute the bounding box for the contour, draw it on the frame
				# and update the text
				largestC = cArea
				(x, y, w, h) = cv2.boundingRect(c)
				cv2.rectangle(image, (x, y), (x + w, y + h), (0, 255, 0), 2)
				text = "Target acquired"
				font = cv2.FONT_HERSHEY_SIMPLEX
				cv2.putText(image,text, (x, y), font, 1, (255, 255, 255), 2)
				COM = getCOM(contours)
	
		#cv2.imshow("RAW", imageRaw)
		#cv2.imshow("Threshold", mask)
		#cv2.imshow("overlay",image) #Show what camera is seeing as overlay
		key = cv2.waitKey(1) & 0xF
	
    	# clear the stream in preparation for the next frame
		rawCapture.truncate(0)
		
		# if the `q` key was pressed, break from the loop
		
		#if key == ord("q"):
			#break

		return (COM, largestC)

def cleanup():
	# cleanup the wheels

	print("cleanup")
	global p1
	global p2

	p1.stop()
	p2.stop()


def init_wheels():
	# initialize wheels

	print("init wheels")
	global p1
	global p2

	#PWM Code
	# Initialize Servos
	#p1 = GPIO.PWM(12, 46.51163)
	#p2 = GPIO.PWM(16, 46.51163)
	p1.start(0)
	p2.start(0)

	p1.ChangeDutyCycle(0)
	p2.ChangeDutyCycle(0)

def NavMain():
	# main navigation function
	global CamInit
	print("start")
	
	# only initialize the camera once
	if CamInit == False:
		init_camera()
		CamInit = True
	init_wheels()
	#image_process()
	nav()
	cleanup()

# Button 27 pressed. Bail Out (quit)
def GPIO17_callback(channel):
	global Q
	Q = True
GPIO.add_event_detect(17,GPIO.FALLING, callback=GPIO17_callback,bouncetime=300)

# Read RFID polling output to obtain tag's UID as a string
def ScanRFID():
	global RFID_UID
	global RFID_found
	global NFC_data
	RFID_found = False #initialize
	
	NFC_txt = open("/home/pi/RFID_Project/NFC.txt", "r")
	NFC_data = NFC_txt.read() #Reads contents of NFC.txt and converts string
	NFC_txt.close()
	#print(NFC_data[203:207]) #from ip.txt ATQA
	#print(NFC_data[232:236]) #from bash script made fifo ATQA
	print(NFC_data[96:111]) #from RFID_simple_read made fifo ATQA
	#NFC_data_char = list(NFC_data)
	#print(NFC_data_char([2,40])
	#print(NFC)
	if NFC_data[49:53] == "ATQA": #RFID_simple_read will only return an ATQA value if it finds a tag
		print("RFID Detected!")
		RFID_found = True
		#print("UID = " + NFC_data[250:264]) #from ip.txt UID
		#print("UID = " + NFC_data[279:294]) #from bash script made fifo UID
		print("UID = " + NFC_data[96:111]) #from RFID_simple_read made fifo UID
		RFID_UID = NFC_data[96:111]
	else: #If RFID_simple_read did not return an ATQA value, then it threw an error
		print("RFID could not be found!") 
		RFID_found = False
RFID_UID = 0 #Define RFID_UID to prevent undefined variable error later

# PyGame Initialization Code
pygame.init()
pygame.mouse.set_visible(False) # Make mouse invisible on TFT
BLACK = 0, 0, 0
WHITE = 255, 255, 255
screen = pygame.display.set_mode((320,240))

# Load Object <=> UID assignments from file if they exist
# Object_Def.csv has the dictionary keys in the first column and values in
# second column

# if Object_Def.csv exists:
if os.path.isfile("/home/pi/RFID_Project/Object_Def.csv"):
	print("Yes FILE!")
	with open("/home/pi/RFID_Project/Object_Def.csv", "r") as infile:
		reader = csv.reader(infile)
		Object_Def = {rows[0]:rows[1] for rows in reader}
else:
	print("NO Existing Object Library FILE!")

print(Object_Def) #TEST

##### Initialize Font Parameters and Menu Text #####
my_font20 = pygame.font.Font(None,20) # Fontsize=20
my_font25 = pygame.font.Font(None,25) # Fontsize=25
my_font30 = pygame.font.Font(None,30) # Fontsize=30
my_font40 = pygame.font.Font(None,40) # Fontsize=40
Object_List_Updated = False
text_back_botright = {(240,200):'Back'}

text_object_overview = {(30,30):'Name:',(30,60):'UID:',(100,30):'#NAME#',(100,60):'#UID_HERE#'}
text_object_nameonly = {(30,30):'Name:',(100,30):'#NAME#',(30,60):'UID:'}

text_page_nav = {(240,60):'Next Pg.',(240,130):'Prev Pg.'}
text_start_menu = {(10,20):'Search for RFID',(10,110):'Write/Overwrite RFID'}
text_start_quit = {(240,200):'Quit'}

text_scan_menu = {(160,20):'Search For RFID Tagged Object'}
text_scan_confirm = {(160,140):'Are you sure you wish to SEARCH for this tag?'}

text_write_menu = {(10,20):'New/Overwrite RFID Tag',(10,80):'Rename Tag Object',(10,140):'Delete RFID from Database'}
#text_write_back_clearall = {(240,200):'Back',(10,200):'Clear ALL'}
text_write_back_clearall = {(240,200):'Back'}
text_write_overwrite = {(160,20):'Rename Existing RFID Tag'}
text_write_delete = {(160,20):'Delete Existing RFID Tag'}
text_overwrite_confirm = {(160,140):'Are you sure you wish to RENAME this tag?'}
text_delete_confirm = {(160,140):'Are you sure you wish to DELETE this tag?'}
text_already_exists = {(160,100):'ATTENTION!',(160,120):'This tag already has an object assigned to it.',(160,140):'Do you wish to OVERWRITE this tag?'}
text_yes_cancel = {(100,180):'Yes',(220,180):'Cancel'}

text_scanning_stopped = {(160,30):'Scanning stopped!',(160,60):'#REASON#'}
text_scan_tryagain = {(160,140):'Would you like to try again?',(90,180):'Try Again',(230,180):'Main Menu'}

text_scanning_inprogress = {(160,140):'Currently Searching...'}
text_scanning_taptocancel = {(160,180):'Tap anywhere to CANCEL.'} 

text_OK = {(160,180):'OK'}

text_assign_menu = {(160,140):'Hold RFID tag up to scanner NOW.', (160,180):'Tap anywhere to CANCEL.'}

text_assign_success = {(160,140):'RFID tag successfully assigned!'}
text_assign_rename = {(160,140):'RFID tag successfully renamed!'}

text_object_found = {(160,140):'OBJECT FOUND!!!'}
text_object_taptomenu = {(160,200):'Tap anywhere to return to main menu.'}

text_name_nav = {(20,200):'<',(80,200):'^',(140,200):'v',(200,200):'>',(260,70):'Done'}
text_name_slot = {(10,140):'_',(60,140):'_',(110,140):'_',(160,140):'_',(210,140):'_',(260,140):'_'}
text_name_menu = {(20,30):'Name your object using the arrows below.',(20,50):'Tap "Done" when finished.'}
NameDict={(10,130):' ',(60,130):' ',(110,130):' ',(160,130):' ',(210,130):' ',(260,130):' '}

Char_List=[' ','A','B','C','D','E','F','G','H','I','J','K','L','M','N','O','P','Q','R','S','T','U','V','Q','X','Y','Z','1','2','3','4','5','6','7','8','9','0','*']


List_Page = 1
MENU = 'Start1'
LIST_MENU = 'Search'


Q = False
Scanning = False
object_chosen = False
OverwriteChoice=False
TryScanner = True
Shutdown=False

#################### Primary "While" Polling Loop ####################
while Q==False:
	time.sleep(0.2)
	screen.fill(BLACK)

##### Update Object Database Display List #####
# If the Object_Def dictionary has been updated, indicating a change in UID or object name,
# then update the text dictionary to properly display the updates in the on-screen list

	if Object_List_Updated == False and bool(Object_Def) and ('Object_Def' in locals()): # If the object list needs to be updated, Object_Def is nonempty, and exists
		object_list = list(Object_Def.keys())
		num_objects = len(object_list)
		if num_objects <= 8: # Thus will not require multiple pages
			object_text1 = {}
			if num_objects <= 4: # Thus will only require one row
				for i in range(0,num_objects):
					object_text1[(10,60+(50*i))]=(str(i+1) + '. ')
					object_text1[(30,60+(50*i))]= (object_list[i])
			else: # Thus will require two rows
				for i in range(0,4):
					object_text1[(10,60+(50*i))]= (str(i+1) + '. ')
					object_text1[(30,60+(50*i))]= (object_list[i])
				for j in range(4,num_objects):
					object_text1[(110,60+(50*(j-4)))]= (str(j+1) + '. ')
					object_text1[(130,60+(50*(j-4)))]= (object_list[j])
		elif num_objects > 8: # Will require multiple pages (Max of 3 pages)
			pages = int(math.ceil(float(num_objects)/8))
			page_rows = int(math.ceil(float(num_objects)/4))
			object_text1 = {}
			object_text2 = {}
			object_text3 = {}
			for i in range(0,page_rows):
				if (i+1) <= 2: #If on first page
					if (i+1) % 2 ==0: #If row number is even, put on right
						if i == page_rows-1: #If last row
							for j in range(4*i,num_objects):
								object_text1[(110,60+(50*(j-(4*i))))]= (str(j+1) + '. ')
								object_text1[(130,60+(50*(j-(4*i))))]= (object_list[j])
						else:
							for j in range(4*i,4+(4*i)):
								object_text1[(110,60+(50*(j-(4*i))))]= (str(j+1) + '. ')
								object_text1[(130,60+(50*(j-(4*i))))]= (object_list[j])
					else: #If row number is odd, put on left
						if i == page_rows-1: # If last row
							for j in range(4*i,num_objects):
								object_text1[(10,60+(50*(j-(4*i))))]= (str(j+1) + '. ')
								object_text1[(30,60+(50*(j-(4*i))))]= (object_list[j])
								
						else:
							for j in range(4*i,4+(4*i)):
								object_text1[(10,60+(50*(j-(4*i))))]= (str(j+1) + '. ')
								object_text1[(30,60+(50*(j-(4*i))))]= (object_list[j])
				elif (i+1) > 2 and (i+1)<=4: #If on second page
					if (i+1) % 2 ==0: #If row number is even, put on right
						if i == page_rows-1: #If last row
							for j in range(4*i,num_objects):
								object_text2[(110,60+(50*(j-(4*i))))]= (str(j+1) + '. ')
								object_text2[(130,60+(50*(j-(4*i))))]= (object_list[j])
						else:
							for j in range(4*i,4+(4*i)):
								object_text2[(110,60+(50*(j-(4*i))))]= (str(j+1) + '. ')
								object_text2[(130,60+(50*(j-(4*i))))]= (object_list[j])
					else: #If row number is odd, put on left
						if i == page_rows-1: # If last row
							for j in range(4*i,num_objects):
								object_text2[(10, 60+(50*(j-(4*i))))]= (str(j+1) + '. ')
								object_text2[(30, 60+(50*(j-(4*i))))]= (object_list[j])
								
						else:
							for j in range(4*i,4+(4*i)):
								object_text2[(10, 60+(50*(j-(4*i))))]= (str(j+1) + '. ')
								object_text2[(30, 60+(50*(j-(4*i))))]= (object_list[j])
				elif (i+1) > 4 and (i+1)<=6: #If on third page
					if (i+1) % 2 ==0: #If row number is even, put on right
						if i == page_rows-1: #If last row
							for j in range(4*i,num_objects):
								object_text3[(110,60+(50*(j-(4*i))))]= (str(j+1) + '. ')
								object_text3[(130,60+(50*(j-(4*i))))]= (object_list[j])
						else:
							for j in range(4*i,4+(4*i)):
								object_text3[(110,60+(50*(j-(4*i))))]= (str(j+1) + '. ')
								object_text3[(130,60+(50*(j-(4*i))))]= (object_list[j])
					else: #If row number is odd, put on left
						if i == page_rows-1: # If last row
							for j in range(4*i,num_objects):
								object_text3[(10, 60+(50*(j-(4*i))))]= (str(j+1) + '. ')
								object_text3[(30, 60+(50*(j-(4*i))))]= (object_list[j])
								
						else:
							for j in range(4*i,4+(4*i)):
								object_text3[(10, 60+(50*(j-(4*i))))]= (str(j+1) + '. ')
								object_text3[(30, 60+(50*(j-(4*i))))]= (object_list[j])
		Object_List_Updated=True
		
		# Save Object_Def to .csv file before exit
		File = open("Object_Def.csv", "w")
		SaveObjectDict = csv.writer(File)
		for key, val in Object_Def.items():
			SaveObjectDict.writerow([key, val])
		File.close()

############################################################
##### Render User TFT Menus #####
	# Render Text Based on Current Menu (Defined by MENU variable)
	if MENU == 'Start1': 
		for text_pos, my_text in text_start_menu.items():
			text_surface = my_font40.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
		for text_pos, my_text in text_start_quit.items():
			text_surface = my_font20.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
	elif MENU == 'Write':
		for text_pos, my_text in text_write_menu.items():
			text_surface = my_font30.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
		for text_pos, my_text in text_write_back_clearall.items():
			text_surface = my_font20.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
	elif MENU == 'List':
		for text_pos, my_text in text_page_nav.items():
			text_surface = my_font20.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
		for text_pos, my_text in text_back_botright.items():
			text_surface = my_font20.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
		
		if LIST_MENU == 'Overwrite':
			for text_pos, my_text in text_write_overwrite.items():
				text_surface = my_font25.render(my_text,True,WHITE)
				rect = text_surface.get_rect(center=text_pos)
				screen.blit(text_surface,rect)
		elif LIST_MENU == 'Delete':
			for text_pos, my_text in text_write_delete.items():
				text_surface = my_font25.render(my_text,True,WHITE)
				rect = text_surface.get_rect(center=text_pos)
				screen.blit(text_surface,rect)
		elif LIST_MENU == 'Search':
			for text_pos, my_text in text_scan_menu.items():
				text_surface = my_font25.render(my_text,True,WHITE)
				rect = text_surface.get_rect(center=text_pos)
				screen.blit(text_surface,rect)
		
		if List_Page == 1: 
			for text_pos, my_text in object_text1.items():
				text_surface = my_font20.render(my_text,True,WHITE)
				rect = text_surface.get_rect(topleft=text_pos)
				screen.blit(text_surface,rect)
		elif List_Page == 2: 
			for text_pos, my_text in object_text2.items():
				text_surface = my_font20.render(my_text,True,WHITE)
				rect = text_surface.get_rect(topleft=text_pos)
				screen.blit(text_surface,rect)
		elif List_Page == 3: 
			for text_pos, my_text in object_text3.items():
				text_surface = my_font20.render(my_text,True,WHITE)
				rect = text_surface.get_rect(topleft=text_pos)
				screen.blit(text_surface,rect)
	elif MENU == 'Confirm': 
		if CONFIRM_MENU == 'Overwrite_Confirm':
			for text_pos, my_text in text_overwrite_confirm.items():
				text_surface = my_font20.render(my_text,True,WHITE)
				rect = text_surface.get_rect(center=text_pos)
				screen.blit(text_surface,rect)
		elif CONFIRM_MENU == 'Delete_Confirm':
			for text_pos, my_text in text_delete_confirm.items():
				text_surface = my_font20.render(my_text,True,WHITE)
				rect = text_surface.get_rect(center=text_pos)
				screen.blit(text_surface,rect)
		elif CONFIRM_MENU == 'Search_Confirm':
			for text_pos, my_text in text_scan_confirm.items():
				text_surface = my_font20.render(my_text,True,WHITE)
				rect = text_surface.get_rect(center=text_pos)
				screen.blit(text_surface,rect)
		elif CONFIRM_MENU == 'AlreadyExists_Confirm':
			#Update display for chosen tag information
			text_object_overview[(100,30)]=ExistName
			text_object_overview[(100,60)]=RFID_UID
			for text_pos, my_text in text_already_exists.items():
				text_surface = my_font20.render(my_text,True,WHITE)
				rect = text_surface.get_rect(center=text_pos)
				screen.blit(text_surface,rect)
		for text_pos, my_text in text_yes_cancel.items():
			text_surface = my_font20.render(my_text,True,WHITE)
			rect = text_surface.get_rect(center=text_pos)
			screen.blit(text_surface,rect)
		for text_pos, my_text in text_object_overview.items():
			text_surface = my_font25.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
	elif MENU == 'RFID_Name':
		for text_pos, my_text in text_back_botright.items():
			text_surface = my_font20.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
		for text_pos, my_text in text_name_menu.items():
			text_surface = my_font20.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
		for text_pos, my_text in text_name_nav.items():
			text_surface = my_font25.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
		for text_pos, my_text in text_name_slot.items():
			text_surface = my_font30.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
		for text_pos, my_text in NameDict.items():
			text_surface = my_font30.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
	elif MENU == 'AssignRFID':
		#Update display for chosen tag information
		text_object_nameonly[(100,30)]=NAME_Obj
		for text_pos, my_text in text_assign_menu.items():
			text_surface = my_font20.render(my_text,True,WHITE)
			rect = text_surface.get_rect(center=text_pos)
			screen.blit(text_surface,rect)
		for text_pos, my_text in text_object_nameonly.items():
			text_surface = my_font25.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
		
	elif MENU == 'AssignSuccess':
		#Update display for chosen tag information
		text_object_overview[(100,30)]=NAME_Obj
		text_object_overview[(100,60)]=RFID_UID
		if OverwriteChoice==True:
			for text_pos, my_text in text_assign_rename.items():
				text_surface = my_font20.render(my_text,True,WHITE)
				rect = text_surface.get_rect(center=text_pos)
				screen.blit(text_surface,rect)
		else:
			for text_pos, my_text in text_assign_success.items():
				text_surface = my_font20.render(my_text,True,WHITE)
				rect = text_surface.get_rect(center=text_pos)
				screen.blit(text_surface,rect)
		for text_pos, my_text in text_OK.items():
			text_surface = my_font20.render(my_text,True,WHITE)
			rect = text_surface.get_rect(center=text_pos)
			screen.blit(text_surface,rect)
		for text_pos, my_text in text_object_overview.items():
			text_surface = my_font25.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
	elif MENU == 'ScanningStopped':
		text_scanning_stopped[(160,60)] = ScanCancel #Update reason for cancel
		for text_pos, my_text in text_scanning_stopped.items():
			text_surface = my_font25.render(my_text,True,WHITE)
			rect = text_surface.get_rect(center=text_pos)
			screen.blit(text_surface,rect)
		for text_pos, my_text in text_scan_tryagain.items():
			text_surface = my_font20.render(my_text,True,WHITE)
			rect = text_surface.get_rect(center=text_pos)
			screen.blit(text_surface,rect)
	elif MENU == 'SearchInProgress':
		for text_pos, my_text in text_scanning_inprogress.items():
			text_surface = my_font25.render(my_text,True,WHITE)
			rect = text_surface.get_rect(center=text_pos)
			screen.blit(text_surface,rect)
		for text_pos, my_text in text_scanning_taptocancel.items():
			text_surface = my_font20.render(my_text,True,WHITE)
			rect = text_surface.get_rect(center=text_pos)
			screen.blit(text_surface,rect)
		for text_pos, my_text in text_object_overview.items():
			text_surface = my_font25.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
	elif MENU == 'ObjectFound':
		for text_pos, my_text in text_object_found.items():
			text_surface = my_font30.render(my_text,True,WHITE)
			rect = text_surface.get_rect(center=text_pos)
			screen.blit(text_surface,rect)
		for text_pos, my_text in text_object_taptomenu.items():
			text_surface = my_font20.render(my_text,True,WHITE)
			rect = text_surface.get_rect(center=text_pos)
			screen.blit(text_surface,rect)
		for text_pos, my_text in text_object_overview.items():
			text_surface = my_font25.render(my_text,True,WHITE)
			rect = text_surface.get_rect(topleft=text_pos)
			screen.blit(text_surface,rect)
	
	pygame.display.flip()
	
########## Assign RFID Tag ##########
# If AssignRFID is chosen, this code section will scan for tags and assign to it the name provided
# by the user, then update the system's Object Definition database. If it does not detect a tag
# after 10 seconds, it will cancel and ask the user if they wish to try again.

	if MENU == 'AssignRFID':
		TryScanner=True
		while TryScanner==True: #Restarts scanner if scanner fell asleep and could not detect object
			cmd = 'sudo /home/pi/RFID_Project/RFID_simple_read_v2 > /home/pi/RFID_Project/NFC.txt'
			ScanProcess = subprocess.Popen(cmd, shell=True) #Start scanning for RFID tags
			Scanning=True
			if Scanning==True:
				ScanStartTime = time.time() #Note starttime for 10 second timeout
				while Scanning==True:
					time.sleep(0.2)
					ScanProcess.poll() #check if still scanning. Returns NONE if still running ScanProcess
					for event in pygame.event.get(): #For cancel button
						if (event.type is MOUSEBUTTONDOWN): #If mouse button clicked down
							pos = pygame.mouse.get_pos()
						elif (event.type is MOUSEBUTTONUP): #If mouse button is released
							pos = pygame.mouse.get_pos()
							x,y = pos
							if (x>0 and y>0):
								ScanProcess.kill() #Stop scanning
								Scanning=False
								TryScanner=False
								LastMENU = MENU
								print("Scanning Canceled")
								ScanCancel = 'RFID assignment canceled by user.' #Update reason for stop
								MENU = 'ScanningStopped'
					if ScanProcess.poll() == None:
						print("Still scanning")
						if (time.time() - ScanStartTime)>10: #Time-Out after 10 seconds
							ScanProcess.kill() #Stop scanning
							Scanning=False
							TryScanner=False
							LastMENU = MENU #Make note of last action if user wishes to try again
							ScanCancel = 'Took too long! Assignment timeout.' #Update reason for stop
							MENU = 'ScanningStopped'
							print('TOOK TOO LONG! Scanning Stopped.')
					else: #RFID scanner stopped scanning
						print("No longer scanning")
						Scanning=False
						TryScanner=False
						ScanRFID()
						if RFID_found==False: #RFID scanner stopped because of error
							TryScanner=True #Try starting scanner again
						elif RFID_found==True: #RFID scanner stopped because it found tag
							if RFID_UID in Object_Def.values(): #If tag already assigned
								for k in Object_Def.keys(): #Find obj currently associated with UID
									if Object_Def[k]==RFID_UID:
										ExistName= k
										
								MENU = 'Confirm'
								CONFIRM_MENU = 'AlreadyExists_Confirm'
							else: #RFID tag detected and does not already exist. Proceed with assignment
								Object_Def[NAME_Obj] = RFID_UID
								Object_List_Updated=False
								MENU='AssignSuccess'
				#Empty NameDict Once More
				NameDict={(10,130):' ',(60,130):' ',(110,130):' ',(160,130):' ',(210,130):' ',(260,130):' '}

##### Search In Progress (Robot Moving) #####
	if MENU == 'SearchInProgress':
		NavMain()
	
##### Detect Touchscreen Input For Onscreen Buttons #####
	for event in pygame.event.get():
		if (event.type is MOUSEBUTTONDOWN): #If mouse button clicked down
			pos = pygame.mouse.get_pos()
		elif (event.type is MOUSEBUTTONUP): #If mouse button is released
			pos = pygame.mouse.get_pos()
			x,y = pos
			# Touchscreen Button Definitions
			if MENU == 'Start1':
				#If on start (first) screen
				if (y<100): #Search For RFID
					print('Go to Scanning Menu')
					MENU = 'List'
					LIST_MENU = 'Search'
				elif (y>100 and y<200): #Write/Overwrite RFID pressed
					MENU = 'Write'
				elif (x>240 and y>200): #Quit
					Q=True
					#Shutdown=True #Will shutdown Pi on quit if left uncommented
			elif MENU == 'Write':
				#If on Write menu
				if (y<70): #New Object
					print('Write new RFID tag. Go to MENU=RFID_Name')
					Char1Pos=0
					Char2Pos=0
					Char3Pos=0
					Char4Pos=0
					Char5Pos=0
					Char6Pos=0
					NAME_CHAR=1
					MENU='RFID_Name'
				elif (y>70 and y<130): #Overwrite existing object
					print('Overwrite RFID tag')
					MENU = 'List'
					LIST_MENU = 'Overwrite'
				elif (y>130 and y<200): #Delete RFID/Object
					print('Delete RFID tag from database')
					MENU = 'List'
					LIST_MENU = 'Delete'
				elif (x>240 and y>200): #Back
					MENU = 'Start1'


			elif MENU == 'RFID_Name': #Menu for assigning new name
				if (x<50 and y>190): #Left (<) pressed
					if NAME_CHAR==1:
						NAME_CHAR=6
					else:
						NAME_CHAR = NAME_CHAR-1
				elif (x>50 and x<110 and y>190): #Up (^) pressed
					if NAME_CHAR==1:
						if Char1Pos == len(Char_List)-1:
							Char1Pos = 0 #Cycle through to first char
						else:
							Char1Pos = Char1Pos+1 #Cycle to next char
						NameDict[(10,130)]=Char_List[Char1Pos]
					if NAME_CHAR==2:
						if Char2Pos == len(Char_List)-1:
							Char2Pos = 0 #Cycle through to first char
						else:
							Char2Pos = Char2Pos+1 #Cycle to next char
						NameDict[(60,130)]=Char_List[Char2Pos]
					if NAME_CHAR==3:
						if Char3Pos == len(Char_List)-1:
							Char3Pos = 0 #Cycle through to first char
						else:
							Char3Pos = Char3Pos+1 #Cycle to next char
						NameDict[(110,130)]=Char_List[Char3Pos]
					if NAME_CHAR==4:
						if Char4Pos == len(Char_List)-1:
							Char4Pos = 0 #Cycle through to first char
						else:
							Char4Pos = Char4Pos+1 #Cycle to next char
						NameDict[(160,130)]=Char_List[Char4Pos]
					if NAME_CHAR==5:
						if Char5Pos == len(Char_List)-1:
							Char5Pos = 0 #Cycle through to first char
						else:
							Char5Pos = Char5Pos+1 #Cycle to next char
						NameDict[(210,130)]=Char_List[Char5Pos]
					if NAME_CHAR==6:
						if Char6Pos == len(Char_List)-1:
							Char6Pos = 0 #Cycle through to first char
						else:
							Char6Pos = Char6Pos+1 #Cycle to next char
						NameDict[(260,130)]=Char_List[Char6Pos]
				elif (x>110 and x<170 and y>190): #Down (v) pressed
					if NAME_CHAR==1:
						if Char1Pos == 0:
							Char1Pos = len(Char_List)-1 #Cycle to last char
						else:
							Char1Pos = Char1Pos-1 #Cycle to prev char
						NameDict[(10,130)]=Char_List[Char1Pos]
					if NAME_CHAR==2:
						if Char2Pos == 0:
							Char2Pos = len(Char_List)-1 #Cycle to last char
						else:
							Char2Pos = Char2Pos-1 #Cycle to prev char
						NameDict[(60,130)]=Char_List[Char2Pos]
					if NAME_CHAR==3:
						if Char3Pos == 0:
							Char3Pos = len(Char_List)-1 #Cycle to last char
						else:
							Char3Pos = Char3Pos-1 #Cycle to prev char
						NameDict[(110,130)]=Char_List[Char3Pos]
					if NAME_CHAR==4:
						if Char4Pos == 0:
							Char4Pos = len(Char_List)-1 #Cycle to last char
						else:
							Char4Pos = Char4Pos-1 #Cycle to prev char
						NameDict[(160,130)]=Char_List[Char4Pos]
					if NAME_CHAR==5:
						if Char5Pos == 0:
							Char5Pos = len(Char_List)-1 #Cycle to last char
						else:
							Char5Pos = Char5Pos-1 #Cycle to prev char
						NameDict[(210,130)]=Char_List[Char5Pos]
					if NAME_CHAR==6:
						if Char6Pos == 0:
							Char6Pos = len(Char_List)-1 #Cycle to last char
						else:
							Char6Pos = Char6Pos-1 #Cycle to prev char
						NameDict[(260,130)]=Char_List[Char6Pos]
				elif (x>170 and x<230 and y>190): #Right (>) pressed
					if NAME_CHAR==6:
						NAME_CHAR=1
					else:
						NAME_CHAR = NAME_CHAR+1
				elif (x>210 and y>50 and y<110): #Done pressed
					NewNAME_Obj = str(NameDict[(10,130)]+NameDict[(60,130)]+NameDict[(110,130)]+NameDict[(160,130)]+NameDict[(210,130)]+NameDict[(260,130)])
					print('Go to MENU=AssignRFID')
					MENU = 'AssignRFID'
					Scanning = True
					if OverwriteChoice==True: #If overwriting name of existing tag
						Object_Def[NewNAME_Obj] = Object_Def.pop(NAME_Obj)
						NAME_Obj = NewNAME_Obj
						RFID_UID = Object_Def[NewNAME_Obj]
						MENU = 'AssignSuccess'
						Object_List_Updated=False #Update object list
					else: #Make note of chosen name for tag assignment
						NAME_Obj = NewNAME_Obj

					#Empty NameDict Once More
					NameDict={(10,130):' ',(60,130):' ',(110,130):' ',(160,130):' ',(210,130):' ',(260,130):' '}
					print('Object Name: ' + NAME_Obj)
				elif (x>240 and y>190): #Back pressed
					if OverwriteChoice==True:
						MENU='List'
						LIST_MENU='Overwrite'
						OverwriteChoice=False
					else:
						MENU = 'Write' 
					#Empty NameDict Once More
					NameDict={(10,130):' ',(60,130):' ',(110,130):' ',(160,130):' ',(210,130):' ',(260,130):' '}


			elif MENU == 'AssignSuccess': #MENU for after successful RFID assignment
				if (x>0 and y>0):
					MENU = 'Start1'
					if OverwriteChoice==True:
						OverwriteChoice=False


			elif MENU == 'ScanningStopped': #MENU for if script was scanning for RFID, but was canceled before finding a tag
				if (x>160 and y>170): #Main Menu
					MENU = 'Start1'
				elif (x<140 and y>170): #Try Again
					MENU = LastMENU #Retry action before stop

			elif MENU == 'ObjectFound': #MENU for after the robot has found the tag it was looking for
				if (x>0 and y>0):
					MENU = 'Start1'
					#Stop playing sound
					SongProcess.stdin.write('q')

			elif MENU == 'List': #MENU for the 3 page list of current tags in database
				#Detect which entry in the list is chosen by user:
				if (x<100 and y>50 and y<100) and (1+(8*(List_Page-1)))<=num_objects:
					NAME_Obj = object_list[0+(8*(List_Page-1))]
					UID_Obj = Object_Def[object_list[0+(8*(List_Page-1))]]
					print(UID_Obj)
					object_chosen = True
				elif (x<100 and y>100 and y<150) and (2+(8*(List_Page-1)))<=num_objects:
					NAME_Obj = object_list[1+(8*(List_Page-1))]
					UID_Obj = Object_Def[object_list[1+(8*(List_Page-1))]]
					print(UID_Obj)
					object_chosen = True
				elif (x<100 and y>150 and y<200) and (3+(8*(List_Page-1)))<=num_objects:
					NAME_Obj = object_list[2+(8*(List_Page-1))]
					UID_Obj = Object_Def[object_list[2+(8*(List_Page-1))]]
					print(UID_Obj)
					object_chosen = True
				elif (x<100 and y>200 and y<250) and (4+(8*(List_Page-1)))<=num_objects:
					NAME_Obj = object_list[3+(8*(List_Page-1))]
					UID_Obj = Object_Def[object_list[3+(8*(List_Page-1))]]
					print(UID_Obj)
					object_chosen = True
				elif (x>100 and x<200 and y>50 and y<100) and (5+(8*(List_Page-1)))<=num_objects:
					NAME_Obj = object_list[4+(8*(List_Page-1))]
					UID_Obj = Object_Def[object_list[4+(8*(List_Page-1))]]
					print(UID_Obj)
					object_chosen = True
				elif (x>100 and x<200 and y>100 and y<150) and (6+(8*(List_Page-1)))<=num_objects:
					NAME_Obj = object_list[5+(8*(List_Page-1))]
					UID_Obj = Object_Def[object_list[5+(8*(List_Page-1))]]
					print(UID_Obj)
					object_chosen = True
				elif (x>100 and x<200 and y>150 and y<200) and (7+(8*(List_Page-1)))<=num_objects:
					NAME_Obj = object_list[6+(8*(List_Page-1))]
					UID_Obj = Object_Def[object_list[6+(8*(List_Page-1))]]
					print(UID_Obj)
					object_chosen = True
				elif (x>100 and x<200 and y>200 and y<250) and (8+(8*(List_Page-1)))<=num_objects:
					NAME_Obj = object_list[7+(8*(List_Page-1))]
					UID_Obj = Object_Def[object_list[7+(8*(List_Page-1))]]
					print(UID_Obj)
					object_chosen = True
				elif (x>240 and y>190): #Back (to prev menu)
					if LIST_MENU=='Overwrite' or LIST_MENU=='Delete':
						MENU='Write'
					elif LIST_MENU=='Search':
						MENU='Start1'

				if List_Page == 1:
					if (x>230 and y<100): # Next page pressed, go to page 2 from page 1
						List_Page = 2
				elif List_Page == 2:
					if (x>230 and y<100): # Next Page pressed, go to page 3 from page 2
						List_Page = 3
					elif (x>230 and y>100 and y<190): # Prev Page pressed, go to page 1 from page 2
						List_Page = 1
				elif List_Page == 3:
					if (x>230 and y>100 and y<190): # Prev Page pressed, go to page 2 from page 3
						List_Page = 2
				
				if object_chosen == True: #If an entry in the list was chosen
					text_object_overview[(100,30)]=NAME_Obj
					text_object_overview[(100,60)]=UID_Obj
					MENU='Confirm'
					object_chosen=False

					#Go to different confirmation menus based on menu where entry was chosen 
					if LIST_MENU=='Overwrite':
						CONFIRM_MENU='Overwrite_Confirm'
					
					elif LIST_MENU=='Delete':
						CONFIRM_MENU='Delete_Confirm'
					
					elif LIST_MENU=='Search':
						CONFIRM_MENU='Search_Confirm'

			elif MENU=='Confirm': #MENU for confirming various actions
				if CONFIRM_MENU=='AlreadyExists_Confirm': #If user tries to assign a new name to a tag already present in database, ask if they wish to overwrite name associated with tag
					if (x>160 and y>170): #Cancel
						MENU = 'AssignRFID'
					elif (x<140 and y>170): #Confirm (Yes)
						print('Overwrite Tag')
						Object_Def[NAME_Obj] = RFID_UID
						del Object_Def[ExistName] #delete old name definition
						Object_List_Updated=False
						MENU='AssignSuccess'
						
				else:
					if (x>160 and y>170): #Cancel
						MENU = 'List'
					elif (x<140 and y>170): #Confirm (Yes)
						if CONFIRM_MENU=='Overwrite_Confirm':
							print('Overwrite Tag. Go to MENU=AssignRFID')
							OverwriteChoice=True
							Char1Pos=0
							Char2Pos=0
							Char3Pos=0
							Char4Pos=0
							Char5Pos=0
							Char6Pos=0
							NAME_CHAR=1
							MENU = 'RFID_Name'
						elif CONFIRM_MENU=='Delete_Confirm':
							print('Delete Tag')
							del Object_Def[NAME_Obj] #delete item from dict
							Object_List_Updated=False
							MENU = 'List'
						elif CONFIRM_MENU=='Search_Confirm':
							print('Begin searching. Go to MENU=SearchInProgress')
							Search_UID = UID_Obj
							Scanning=True
							MENU = 'SearchInProgress'

# Save Object_Def to .csv file before exit
File = open("Object_Def.csv", "w")
SaveObjectDict = csv.writer(File)
for key, val in Object_Def.items():
	SaveObjectDict.writerow([key, val])
File.close()

##### Cleanup Before Exit #####
p1.stop()
p2.stop()
GPIO.cleanup()
if Shutdown==True: #If Shutdown=True (can occur via quit through TFT if uncommented), shutdown
	ShutdownCMD = 'sudo shutdown -h now'
	subprocess.call(ShutdownCMD, shell=True)
camera.close() #close/cleanup PiCamera


StartUpSleep.py

# dnd37_amc578
# May 14, 2018
# StartUpSleep.py

# Sleeps for 5 seconds to allow for boot initialization in RFID_Track bash script

import time
time.sleep(5)


RFID_Track bash script

# dnd37_amc578
# May 14, 2018
# RFID_Track
#

#!/bin/bash
# ^Run as bash script

# Runs our RFID_Tracking python script RFID_Detect_Main.py but allows for enough
# time to initialize pygame using StartUpSleep.py, allowing smooth startup
# from rc.local

# Sleep for 5 seconds:
sudo python /home/pi/RFID_Project/StartUpSleep.py

# Start main RFID tracking code:
sudo python /home/pi/RFID_Project/RFID_Detect_Main.py


Edited /etc/rc.local

#!/bin/sh -e
#
# /etc/rc.local
#
# This script is executed at the end of each multiuser runlevel.
# Make sure that the script will "exit 0" on success or any other
# value on error.
#
# In order to enable or disable this script just change the execution
# bits.
#
# By default this script does nothing.

# Print the IP address
_IP=$(hostname -I) || true
if [ "$_IP" ]; then
  printf "My IP address is %s\n" "$_IP"
fi

# disable console blanking on PiTFT
sudo sh -c "TERM=linux setterm -blank 0 >/dev/tty0"

# Start RFID Tracking Program on Startup:
# RFID_Track waits 5 seconds for boot to finish, then runs RFID_Detect_Main.py
sudo /home/pi/RFID_Project/RFID_Track &

exit 0