Vision Controlled NIOS-Bot ViCoN-Bot

Jeff Vickers gte613i

Kay Bosompem gte141h

Kevin Walker gte143x

Andre Moore gt6875a

ECE 4006: Senior Design Professor: James Hamblen

Summer 2002

July 25, 2002

Georgia Institute of Technology School of Electrical & Computer Engineering

Table of Contents

EXECUTIVE SUMMARY .................................................................................. - 1 -

INTRODUCTION ................................................................................................. - 2 -

OVERVIEW .......................................................................................................... - 2 -

DIGITAL CAMERA ............................................................................................ - 3 -

NIOS & CMUCAM INTERFACE ...................................................................... - 8 -

PULSE MODULATION WITH THE NIOS BOARD ................................... - 16 -

ROBOT DESIGN ................................................................................................ - 18 -

STATE MACHINE ............................................................................................. - 20 -

TESTING ............................................................................................................ - 23 -

ORGANIZATION .............................................................................................. - 26 -

CONCLUSION ................................................................................................... - 27 -

APPENDIX A: .................................................................................................... A - 29

APPENDIX B:........................................................................................................ B - 33

APPENDIX C:........................................................................................................ C - 54

APPENDIX D:......................................................................................................... D - 69

APPENDIX E:......................................................................................................... E - 80

- 1 -

Executive Summary

The ViCoN-Bot is a name coined for a visually controlled, Nios operated, robot, and was

realized using a digital camera, an Altera Nios FPGA board, and a 1/12 scale electric Hummer.

The mission of the ViCoN-Bot was to traverse a designed course using a digital camera as of

means of detection for a series of unique and defining landmarks. Ideally, the EE hallway

would serve as a full scale course, and future robots would compete in a race based on a design

pioneered by the ViCoN-Bot.

Design of the ViCoN-Bot consisted of research and planning, software development,

hardware implementation, and testing. Extensive research of the camera and its functionality

was conducted to acquire a prior knowledge necessary to develop software controlling the

camera. The software development was comprised primarily of a camera interface, robot speed

and direction control, and a state machine. Hardware execution involved mounting and properly

interconnecting all devices to the robot and providing sufficient power sources for each module.

Integration of the previous phases led to the testing phase. The track consisted of neon pink

landmarks symbolizing forward waypoints, and neon green landmarks denoting when to begin a

full right hand turn. The ViCoN-Bot demonstrated the ability to find and follow the neon pink

markers as well as properly negotiate a turn upon finding neon green. Future projects could

easily build upon the framework set forth with the original design of the ViCoN-Bot. Adding

secondary peripheral devices, improving existing hardware modules, and implementing more

landmarks are among some of the improvements that could be introduced in future projects.

- 2 -

Introduction

The ViCoN-Bot is a standard RC car modified for guidance with a digital camera and

controlled using an FPGA board. Design of the ViCoN-Bot consisted of mastering the control of

a digital camera, programming an interface between the camera and the FPGA, coding a FPGA

module to control the robot, constructing a robot, developing a state machine, assembling a

course, and complete testing and debugging. The goal of the ViCoN-Bot was simply to find and

drive to neon pink landmarks and turn at green landmarks. Accomplishing this would allow for

a unidirectional course to be devised using a series of these landmarks as waypoints. By meeting

this goal, the ViCoN-Bot is certain to lay the foundation for more advanced senior designs that

utilize digital cameras in analogous ways.

Overview

The design of the ViCoN-Bot began with researching and selecting the proper

components and equipment. Due to readily available supply and support locally, the Tamiya

M1025 Hummer and Altera Nios board were the robot and FPGA board of choice. Proceeding

significant research, the CMUcam was elected the most logical choice of a digital camera

offering basic functionality integrated into the unit itself. Complete documentation provided in

the CMUcam manual aided in developing a comprehensive understanding of the camera. Three

battery sources where chosen to power the Hummer, Nios board, and camera. An additional

voltage regulator circuit was implemented to supply the robot with a reference potential. A full

parts list of all basic components is shown below in Table E-1.

The basic design consisted of several units all utilizing the primary processor. The Nios

board provided the logic for both the system processor and additional units designed in C and

VHDL. The logic blocks implemented on the Nios board consisted of a basic driver or state

machine coordinated with the processor, a UART or camera interface, a speed control, and a

direction control unit. The camera interface received data packets from the camera mounted on

top of the robot, output the data to the state machine, and transmitted commands to the camera

from the state machine. The state machine used the data packets to determine the robot’s

orientation with its surrounding environment and compute the proper reaction of the robot. The

computation was recorded in the form of two integer values ranging from 0 to 8, one for speed

and one for direction. Both integers were output to corresponding pulse width modulation,

- 3 -

PWM, control units in the form of a 3-bit number corresponding to eight speed and eight

directions settings. Each control unit deciphered the 3-bit input as a pulse length or duty cycle

and generated a 20 ms output pulse of equivalent proportion. The PWM signals were routed to

the robot’s speed controller which directly controls the robot. A diagram outlining the basic

operation and flow of the ViCoN-Bot is shown below in Figure 1.

Figure 1. Diagram of the ViCoN-Bot’s overall operation and data flow.

Once all functional units were assembled and jointly operational, a track consisting of

chosen objects was developed. Through research of the CMUcam, neon pink and green

rectangular objects were determined to be landmarks for a unidirectional course. Guided by the

CMUcam the Hummer could traverse the course by seeking landmarks and reacting to them as

the state machine was programmed. This arrangement collectively led to the construction of the

ViCoN-Bot.

Digital Camera

After much research, the camera module of choice was a low-cost, low-power CCD

camera developed by Carnegie Mellon. This unit was developed for robotics purposes and was

- 4 -

code named CMUcam as pictured in Figure 2. The camera itself is an Omnivision OV6620,

single-chip CMOS CIF color digital camera. Coupled with an 80 MHz Scenix Programmable

Integrated Circuit or PIC, the unit was capable of preprocessing the image and performing minor

calculations. Processing the raw images instantaneously limited the overall resolution to 80

pixels wide by 143 pixels high and a maximum of 17 frames per second. The CMUcam

communicated on a serial bus and ASCII interface with a computer via a standard telnet

application or a Java GUI packaged with firmware v1.12 of the software for CMUcam. A null-

modem was needed to communicate with the Nios board properly. A ¼” wide angle IR lens was

added to filter out infrared and noisy light emitted from the sun and incandescent lights and also

widened the field of view to 25°. The CMUcam required approximately 6 volts DC and 200

milliamps. Full CMUcam specifications including complete parts list, IC layout, schematic, full

commands, packet formats, and user manual are referenced in the ViCoN-Bot CD.

Figure 2. CMUcam: left is the front view and right is the back view.

The CMUcam required excessive experimentation and testing to properly master its

functionality and understand its reaction to different environments. The first step was finding the

focus point of the IR lens. It was determined by dumping frames that the sharpest focus of the

camera was achieved at precisely 2 ¾ revolutions from completely screwed in. Once sharp

images could be taken, the next challenge was determining the effects that different lighting

conditions had on the image colors. Even with the aid of the IR lens, it was found that the sun

and incandescent lights still had a large effect on the colors of the image; however, without the

filter, images were very bright, blurry, and almost unrecognizable. Figure 3 shows the

difference between the pictures taken the length of the EE hallway and from the same location

facing the windows.

- 5 -

Figure 3. Pictures taken from the same location facing the sun, left, and an incandescently lighted hallway, right.

The most uniform color patterns were obtained in fluorescent light and areas of indirect

light. The CMUcam has some basic commands that allow tweaking of the color settings to an

extent. This is especially important when in tracking mode. Not only were there differences in

colors of different lighting conditions, but also with varying the distance from which the image

was captured. All of this was taken into consideration when choosing color ranges for tracking

objects. At first thought, black and white objects were chosen as test color ranges. Black and

white objects did not provide a good contrast as the camera computes colors bases on an average.

Grey is the naturally the average color in most images and the factor that limited the contrasting

ability of black and white. Listed in order from best to worst, it was determined by

experimentation that neon yellow, neon pink, and neon green had the most defined color ranges

and contrasted well with surrounding colors. However, neon yellow was immediately eliminated

as an option due to the fact that other lights sources, especially incandescent, drastically

interfered causing the object boundaries to become less defined and often random. At distances

varying from three to twelve meters, neon pink, green, and yellow colored posters were recorded

and viable track ranges were determined from the minimum and maximum values recorded from

each color. The experimental data used to compute the color ranges are stored in Table A-1

through Table A-3. It is notable that neon yellow was not able to produce a traceable image at

twelve meters due to noise from the overhead incandescent lights in the EE hallway. From this

data, it was determined that the best trackable color, neon pink, be used as the color for guidance

landmarks and neon green used as the color for landmarks designating turns.

Once color ranges were determine, the objects used as landmarks were determined

including size, shape, and orientation. Determining the proper landmarks parameters required a

complete knowledge of how the CMUcam tracks objects. Tracking was done with a simple

- 6 -

command and the color range as an input. The CMUcam then searches for the input color range

and calculates the square area that fits that color range. Once the object is located, several

calculations are returned including the coordinates of the two corners defining the object, the

area they encompass, the middle mass of the object, and the confidence in which the object fits

the input color range. The provided GUI shows the object it is tracking with a green, yellow, or

red rectangle and the middle mass with a little red square. The color of the rectangle is

determined by how confident the object is tracking, green being the best. The coordinates are

useful in determining the orientation of the object in the camera’s field of view. The area is

functional in calculating the distance of a known object from the camera. The middle mass

serves as the guiding coordinate for tracking. And the confidence determines how well the

object is tracking as well as means for calculating the speed of the robot. Figure A-1 shows a

picture of two posters before tracking, top, while tracking green, center, and while tracking pink,

bottom, from three to fifteen meters. This figure shows that the neon green poster could be

confidently tracked out to twelve meters and vaguely tracked at fifteen meters. Similarly, the

neon pink poster was tracked confidently at distances exceeding fifteen meters. The ability of

neon pink to be tracked more confidently at greater distances reconfirmed our previous

inclination to use neon pink for the most critical and reoccurring landmark. A half poster of

neon pink was determined to be extremely trackable at distance exceeding ten meters; therefore,

ten meters was chosen to be the distance separating guiding landmarks. A full neon green poster

is used for a land marker denoting a turn.

Once the robot is traversing the course, turning is the next item of concern. Turning

requires a more calculations than mere alignment as with the neon pink guidance markers. The

distance from the turning landmark is crucial in determining when to slow down and begin

turning the robot. Size is the most useful piece of information in calculating distance. Since

landmarks are of known size, color, and location, they can be located and the range determined

by data gathered from the camera. If an object is of know size in an image, the distance could be

calculated precisely; however, the relatively low resolution of the image and high speed of the

robot make this exact calculation less relevant. Rather, it is more useful to predetermine the size

of the image at which a deceleration and turn is appropriate. Size and confidence measurements

were taken from 1 meter to 5.5 meters in ½ meter increments and the data stored in Table A-1.

One major problem seen while recording this data relates to the earlier problem of varying colors

- 7 -

at different distances. Unexpectedly, the size and confidence measurements became increasingly

erratic at distances less than 2.5 meters. Figure 4 demonstrates this by showing the same object

being realized as three different sizes and confidences in different frames while tracking.

Contrary to reason, the CMUcam took more accurate measurements from distances greater than

2.5 meters. In fact, the size was nearly modeled as a linear function of distance in between 2.5

and 5.5 meters. Therefore, the sizes used in negotiating turns will be calculated at distances that

remain in between 2.5 and 5.5 meters.

Figure 4. Tracking errors common associated with close proximities.

Not only does Figure 4 demonstrate an error associated with a combination of close

proximity measurements, interference from an incandescent light source, interference from the

sun, and noise from the green tree in the background, but it also shows the CMUcam’s innate

ability to decipher the true location of the object. Despite several types of inference and sources

of error that could possibly exist in this situation, the middle mass locations are always in

precisely the actual location. This feature demonstrates the rigidity of the CMUcam. Overall,

the CMUcam proved ideal for robotics as the ViCoN-Bot verified.

- 8 -

Nios & CMUcam Interface

After deciding on what camera and robot to use for the ViCoN project, the computing

power required became the final piece of our hardware. This segment is responsible for making

the robot autonomous. A small, lightweight, yet powerful computer with an assortment of

standard peripherals was required. A Field Programmable Gate Array (FPGA) or a

microcontroller would be fairly adequate. Initial research narrowed the list of possibilities for

the ViCoN-Bot to the following:

• Microchip PICF16787

• Microchip PIC18F452

• Microchip PIC18F442

• Xilinx board

• Nios® CPU development board

Based on accessibility, memory size, processor speed and available of peripherals, the Nios®

CPU development board was the most appropriate choice. The Nios® CPU development board

will hitherto be referred to as the Nios board. The Altera Nios board provides all the necessary

tools to create a highly configurable embedded system. This board consists of three basic

components: programmable logic, memory and a processor core. The processor core, after

which the entire system is named, is called Nios. Nios is a customizable soft processor core.

This runs atop an ARM RISC based processor.

Figure 5. Arm based Excalibur Embedded Processor.

- 9 -

Figure 5, shown above, depicts a high level representation of the embedded processor core. All

components in the interior of the core processor can be configured to required specifications.

Figure 6, shown below, illustrates a high level description of the programmable logic device

(PLD). These are the physical dimensions of the complex PLD. It also shows a very important

aspect of the Nios board, the Avalon bus and it’s inter connects. It is the reason why the

processor’s reconfigurations do not skew performance of the Nios board. The bottom right

corner has a User-Defined port where u can create extra UARTs, timers or PIOs.

Figure 6. The Programmable Logic Device.

The Nios board and kit consists of the APEX 20K200E development board, 9-V DC power

supply, a power cable, Serial Y cable, 6-foot 25-pin parallel port extension cable

ByteBlasterMV cable and an LCD Module. The kit also comes with software packages to

make full use of it.

The most significant of the aforementioned software packages are shown under Development

Tools on the left in Figure 7 (shown below). The SOPC (system-on-a-chip) Builder is used to

create new processors and the Quartus® Software is used to embed the processor and any other

custom hardware modules. The SOPC builder consists of a synthesizer and simulator to create

test and fit custom processors.

- 10 -

Figure 7. The Programmable Logic Device Block Diagram.

Figure 8. The Programmable Logic Device IC Layout.

- 11 -

The next few paragraphs will serve as an introduction to the Nios board and how it operates

with emphasis placed on how it was utilized in the ViCoN project. The Apex device (APEX

EP20K200EFC484) below the Flash (ext_flash) chip on the board is used to configure and

evaluate an embedded Nios processor. The Nios Development board had two 64k x 16 SRAM

devices, one 8-Mbit flash memory device and a SODIMM memory expansion socket. The

ViCoN-Bot’s has only I megabyte of flash memory available and below is a break down.

Flash Address Size Comments

0x1C0000 – 0x1FFFFF 256 KByte Factory-default APEX configuration

0x180000 – 0x1BFFFF 256 KByte User-defined APEX configuration data

0x100000 – 0x17FFFF 512 KByte Nios instruction and nonvolatile data space

The MAX7064 device right of the flash (ext_flash) chip and right above the Apex device is a

configuration controller. The configuration controller loads configuration data from the flash

and clocks it into the APEX device. User-created configurations are loaded by shorting JP2

jumpers just below the 9 white switches on the board.

The Nios Development Board comes with a modular plug- in power supply. This 9 volt power

supply plugs into connector J1 (top left of board) and supplies the input power for the on-board

power management. The on-board power management produces the following three voltages:

3.3 volts—Used by the majority of the components on the board, including all APEX IO

banks. 1.8 volts—Used as the core voltage for the APEX EP20K200E device.

5.0 volts—Provided for optional use with the 5V-tolerant Altera daughter card connectors.

The ViCoN-Bot relies on the 5Volt pins available on JP9 and JP11.

The Nios Development Board contains one serial port connector for communications between

the APEX EP20K200E and any external device. The serial port connects via a DB-9 type

connector through an RS-232 transceiver. The board has a 6-pin header at location to assist in

debugging the serial signals.

The Nios Development Board comes with a set of switches and LEDs which proved very

useful. In debugging and testing. Some of the switches are dedicated to resetting the board or

served as indicators of the board’s current status.

- 12 -

The most time saving feature and most utilized aspect of the Nios Board is its generation of a

CPU SDK for any custom system you create. You can then create programs to control or run

on the board. It supports its form of assembly, C and C++.

The ViCoN-Bot project relies heavily on communication between devices. The CMUcam from

Carnegie Mellon University communicates by sending and receiving bytes of ASCII coded

characters through a standard serial port. The Nios CPU deve lopment board responds

similarly. The Nios board also generates and propagates pulse-width modulated signals to the

servo direction and speed motors located in the Tamiya Hummer M1025.

The CMUcam has a serial port for sending bytes of data in response to queries to it. The initial

design of the ViCoN-Bot took this into consideration and simply proposed the serial port as a

point of communications between the camera and Nios board. Our initial test of this

communication link yielded no positive results. Signals sent from either end were not reaching

their destinations. This prompted a research into serial signal transmission and reception and

into the architecture of serial ports and devices. The solution to the signal propagation problem

was to be solved with a male-to-male serial null modem. The design and construction of this

null modem will be discussed later, with specifications included in the robot design section.

ASCII encoded bytes were sent back and forth from the Nios Board to the Camera and the

response displayed on the LCD module. The first ‘successful’ test with the serial null modem

yielded gibberish on the LCD module provided with the Nios development kit. The cause of

this was not easy to ascertain; thus, we assumed the origin of the problem to be from a baud

rate mismatch between the Nios board and CMUcam. The camera’s default and ideal baud rate

is 115200bps and is the same for the Nios board. To verify that 115200kbps was indeed the

default and current setting on both devices, we connected the CMUcam and the Nios board to

different PCs and created mock programs to simulate a Nios-Camera and Camera-Nios

communication. There were no significant observations from these programs, which led us to

create a new theory.

We considered the next most plausible cause: a high baud rate between the devices was not

acceptable to either or both devices. The baud rate was lowered on the camera to its lowest of

- 13 -

9600bps; however, the operation of the camera had deteriorated, become error prone and very

tedious to use. We also discovered the Nios development board' serial port's baud rate could

not be altered in software. The baud rate could only be set alterable by recompiling a custom

processor for the Nios board. This problem was compounded further with the lack of

appropriate licenses to compile a custom processor.

In a bid to utilize time, a third scheme was devised in order to isolate the problem. This

involved delaying commands and responses in software. During this time, we started receiving

familiar ASCII characters, but not what was expected. This partially implied a possible

operation at 115200bps. After doing extensive research on the software and devices, a feasible

and workable problem was found. Our group discovered that the termination character

expected on both devices differed. Our limited use of serial ports were based on PCs. PCs

accept and use both termination characters. This led us to have two choices in reaching a

solution: update the camera’s firmware with a new termination character or have our programs

on the Nios board conform to the camera’s standard and curb the use of standard library calls

when communication with the camera was concerned. The latter presented a safer and more

logical solution. Communication was now established at 115200bps. The next phase of the

Nios-CMUcam interaction now relied on our overall software design.

Programmers of embedded systems do not have the luxury of almost limitless memory and

processor speed their counterparts who program for complete systems of gigabytes of hard disk

memory and hundreds of megabytes of random access memory world enjoy. We need to be

accountable and very meticulous in our software designs. The code for the ViCoN-Bot

incidentally needs to be low level enough to send and receive data at close to real time as

possible.

For the sake of continuity and ease of use for the privileged students who will be continuing

our work on the ViCoN-Bot, we decided to modularize and abstract as much of the code we

deemed necessary and possible. A high level design of our software is depicted below:

- 14 -

Figure 9. ViCoN high level software structure.

vicon_camera.c

In Figure 9 above, the file vicon_camera.c is shown enclosed on the hardware plane CMUcam.

Therefore, all communication between the CMUcam and the Nios Development board is

located in vicon_camera.c. It handles all function calls to the camera taking into account

latencies and necessary data buffering. The CMUcam comes equipped with a set of functions

on its microcontroller. The functions take in 2-letter command (e.g. GV gets firmware version)

or a 2-letter command with a set of numbers (eg TC 0 1 2 3 4 5 tracks colors based on ranges

of colors given by the 6 numbers following it) and returns a set of ASCII coded characters. All

but 2 functions available for the camera were implemented in vicon_camera. The main goal of

this was to simplify the state machine code by abstracting and modularizing the code. Instead

of dealing with every little nuance involved in retrieving some simple data like the camera’s

firmware version, a simple call to the function GetVersion will respond with a simple response.

All functional prototypes and global constants are located in vicon.h

- 15 -

vicon_servo.c

Vicon_servo.c encased in a block with the two motors (Servo Speed and Servo Direction)

handles communication between the Nios board and the motors. Unlike vicon servo above it

only needs to respond to input from the Nios board, more precisely from the state machine. It

indirectly invokes 2 VHDL modules (Speed_control and Direction_Control will be discussed

in the next section) which generate pulse width modulated signals for the direction motor and

speed motor. The peripherals on the Nios board utilized in propagating the pulse width

modulated signal will be discussed later with the custom processor created for the ViCoN-Bot.

vicon.h

Vicon.h holds all function prototypes, global constants, type definitions and structures. This

includes specialized structures representing data packets received from the CMUcam. The

CMUcam is capable of sending five different packet types, namely C, F, M, N and S packets.

Each packet has two variations but only one was deemed useful.

Vicon.c will host the state machine that dictates what the ViCoN-Bot does and how it does it.

The state machine located in vicon.c and the file itself will be discussed in another section.

Nios.h holds all the necessary function prototypes, constructs and structures needed to control

and or communicate with all hardware on the Nios board. A fairly complete C API is

available, but our initial use of these functions proved detrimental to the ViCoN-Bot’s

sensitivity. Two other files shown in the Figure 9 above are pio_lcd16207.c and

pio_lcd16207.h. There hold all the necessary functions and structures necessary to import data

from the Nios board to the LCD module. Initial modifications were undone to keep it standard

for future versions.

In retrospect the entire package could have been done in one huge file but would render the

project difficult to continue or alter. This way any group wishing to use the Nios board and the

CMUcam will have a great resource, vicon_camera.c. Any change to the CMUcam would only

warrant changes in vicon_camera.c. Similarly communication between the Nios board and the

hummer is now ready for further enhancements. A switch to a single motor handling both

speed and direction would also warrant changes only to the file vicon_servo.h.

- 16 -

Pulse Modulation with the Nios Board

Initially, a method of producing pulse width modulations (PWM) was to be determined.

One method considered was to use a pulse width circuit to produce the signal. Another

consideration was to use C or VHDL to program the Nios Board to produce pulse modulation.

It was decided that it would be easier and cheaper to use VHDL to produce the PWM because of

the amount of resources available on how produce PWM in VHDL.

The pulse width design contained two servo controllers in VHDL code named

speed_control.vhd and direction_control.vhd, and a clock generator called clk_div.vhd. The files

can be seen in Appendix C pages 61 through 66. Clk_div.vhd is driven by the Nios 33 MHz

system clock which uses this clock signal to generate various clock speeds. Both servo

controllers are designed to input three-bit numbers that corresponds to a respective speed or

direction. The signals are driven by the 100 kHz clock that is produced by clk_div.vhd. Table

C-1 illustrates how the two servo controls work. For the speed servo each three-bit value

represents 8 settings ranging from stop or, brake, half reverse, neutral, and full forward. A

neutral position is required in order to switch from forward motion to reverse and vice versa.

The in between settings represent fractions of the top speeds desired. Each pulse is between one

and two milliseconds long. The pulses have periodicity of twenty milliseconds; therefore,

speed_control.vhd contains a counter set to count from 0 to 2000 incremented every clock cycle.

The code tests for the pulse widths at the end of each clock cycle as shown in speed_control.vhd.

Direction_control.vhd works the same way as speed_conrtol.vhd except the three-bit number

corresponds to eight direction settings ranging from a 45o left turn to a 45o right turn.

Symbols were generated for the three VHDL files after they were successfully compiled.

The symbols were then connected to the system module as seen in Figure C-1. The output pins

for the pulse modulation were on JP11 on the Nios board, shown in the Figure 10 below. The

red boxes highlight which pins, on JP11, are used to connect to the servo motor pins on the

robot. As shown in the figure pin R20 holds the speed servo three-bit number and pin R1 holds

the direction servo three-bit number. Both of the ground pins were used and the external 5-volt

regulator is used to power the servo.

- 17 -

Figure 10. Output pin assignments on JP11.

After the whole design was successfully compiled, the pulse widths were tested using an

oscilloscope. Figure C-2 and Figure C-4 show the oscilloscope screen captures of a 1.5 ms,

1.75 ms, and 2 ms pulse respectively. Examining these figures, along with the percent error

calculations in Table C-1, illustrates how the actual pulses measured had significantly close duty

cycles to the theoretical values that were calculated. It was concluded from the observations that

the pulse widths were accurate enough to control the robot’s servo motors.

A C source file named vicon_servo.c, shown on pages 67 through 68 of Appendix C, is

used to set the actual speed and direction. Vicon_servo.c contains functions that return an

integer that represents a speed or direction setting. This code is also used in the state-machine to

output this integer to the VHDL servo controllers which then produces the pulse width

modulation.

- 18 -

Robot Design

The ViCoN-Bot was developed from the Tamiya M1025 4X4 Hummer RC hobbyist kit.

The M1025 was equipped with a stock Mabuchi RS-540 27 turn carbon brush DC drive motor

and upgraded with the Futaba MC330CR speed controller. Two 3-pin connectors on the robot

chassis and speed controller are the input signals to the servo for front wheel steering and the

speed controller. Red is the 5 volt reference wire, white is the PWM signal wire, and black is the

ground reference wire.

Assembling the robot proved to be a cumbersome process. The Hummer came equipped

with a removable top hatch which was useful for routing wires. First, the Nios board was

mounted on back end of the Hummer. Holes were drilled in the proper locations and 4X40

screws were used to secure the Nios board to the body. Next, the LCD screen was mounted to

the roof directly in front of the Nios board in a similar fashion. Figure E-1 shows the Nios

board and LCD mounted to the Hummer. Thirdly, the CMUcam was mounted to the roof above

the windshield using small aluminum L-brackets and secured using wing nuts for easy

adjustment as shown in Figure E-2. The camera was aligned by iteratively dumping frames to

the Java GUI and adjusting the camera positions till the image was centered properly. For an

unknown reason, the mounted camera appeared drastically, visually misaligned; however, the

image appeared centered in the bitmap despite the visual displacement.

The CMUcam was powered by four AA 1.5 volt batteries in a series arrangement

providing the required 6 volts. The AA supply was mounted internally underneath the roof, and

had a 2-pin connector that powered the CMUcam through the hatch opening. The M1025 drive

motor and speed controller were powered with a 7.2 volt 2000mAh RC battery and stored in the

Hummer’s built in battery compartment. The Nios board and front servo were powered with an

identical 7.2 volt battery and mounted inside the body, directly underneath the Nios board using

two tie wraps. Figure E-3 shows underneath the hood, the batteries, and chassis of the robot.

Using different power sources for each major component isolated each unit from possible noise

generated by the DC motor or other noise sources while providing extended battery lifetime.

The camera and Nios board shared a common ground that was connected on pin 5 of the serial,

null modem cable. The speed controller and servo had a common ground with the central Nios

board through the ground wire in the 3-pin connectors. Connecting a common reference ground

between each module was essential to proper electrical operation.

- 19 -

Initially, there was minimal wiring involved having only batteries and cables to connect.

First, a null modem cable was created by soldering pins 2, 3, and 5 of two male RS-232 serial

connectors as depicted in Figure 11.

Figure 11. RS-232 Serial Null-Modem wiring diagram

A power connector for the Nios board was then soldered to a connector to the battery and

insulated with heat shrink wrap. It is important to note that the Nios power connector and the

battery connector had different polarities and required that reverse wiring. Next, two twisted 4-

pin computer connectors were used to connect the Nios board to the 3-pin signal wires inside the

robot. The Nios board had only one 5 volt output pin and was used for the LCD screen leaving

no 5 volt reference outputs for the PWM signal connectors. This was solved by constructing a

longer ribbon cable with wires that extended beyond the connector on the Nios board. The

second or red wire on the ribbon cable was determined to be the 5 volt wire. Cutting all the

wires on the ribbon cable except the 5 volt wire left only the 5 volt wire exposed. The orange, 5

volt wire on the 4-pin connector and red 5 volt wire on the Nios board were then stripped,

soldered together, and insulated with heat shrink wrap. The white wires on the 4-pin connectors

were used as the signal wires. Since both connectors were identical, the 4-pin connector for

speed was marked with a yellow dot on each end. The white signal wire of the speed connector

was connected to pin 4 on the J11 of the Nios board, and the green ground wire was connected to

pin 2. Likewise for direction, the white signal wire was connected to pin 40 of J11 and its green

ground wire was connected to pin 36. Finally, all initial wiring was complete and testing was

conducted.

- 20 -

After voltmeter and power up tests, all units appeared to be functioning properly as

originally connected. However, as the ViCoN-Bot progress into the PWM stages, it was

discovered that after powering the drives for a few minutes, the Nios board began flickering and

behaving erratically until it shut off. The amount of current that the servo and speed control unit

required exceeded the rated current of that pin and subsequently caused the Nios board to heat up

until it approached failure. Solving this power limitation complicated the electrical design

somewhat. It was decided that a separate circuit be built to provide the 5 volt reference potential

needed by the servo and speed controller. A 5 volt, ½ amp voltage regulator, a 100 µF

electrolytic capacitor, and a 10 pF polystyrene capacitor where used to construct this circuit. The

two capacitors were put in parallel to decouple the noise induced due to conductive coupling

both at high frequencies bands and low frequencies bands. By bypassing the Nios board with the

voltage regulator circuit, the second battery now provided the PWM reference voltage directly.

Since the Nios board and the voltage regulator share the same ground through a common source,

there was no need for connecting grounds of the two circuits in any other way. The voltage

regulator circuit was mounted about the back right tire, underneath the roof as partial depicted in

Figure E-3.This new arrangement provided sufficient current when fully loaded by the servo and

speed controller; whereas, the Nios board failed under the same conditions.

The ViCoN-Bot was constructed using the M1025 Hummer, Altera Nios board,

CMUcam, several batteries, and a 5 volt regulatory circuit. Pictures shown in Figure E-4 and

Figure E-5 depict a frontal and top view of the final ViCoN-Bot. During the testing phase, a

styrofoam bummer, not shown in the figures, was attached to the front of the robot for protection

against accidental collisions. The final hardware configuration and overall construction of the

robot proved to be more than suitable for the ViCoN-Bot.

State Machine

One of the major milestones in the development of the ViCoN-Bot was complete

communication, passing of information, between the camera and the Nios board. Without a

consistent means of information there would be no sufficient way of accomplishing the overall

goals presented in this design project. The CMUcam communicates through a series of packets

containing pertinent information about the image that it is processing such as the color mean,

coordinates of the tracked color, and size of an image (represented as a pixel value). The packets

- 21 -

from the camera are returned in two different ways the first of which being a constant stream of

packets or a single packet at a time with a sufficient request to the camera. With the ability of

communication between the camera and Nios board the development of a program to control the

action of the ViCoN-Bot could be accomplished. A state machine served as this mechanism of

control.

The state machine created for the robot serves as the internal “traffic-light” for the

ViCoN-Bot. It controlled when packets where sent, how they were processed, and what signal

was sent to the servos. Figure 12 shows an outline of the state machine and how it transitions

from state to state, making decisions for the robot.

Figure 12. A representation of the state machine used for the ViCoN-Bot.

The initial state, CameraStart, sets up the CMUcam for proper use. It initializes the

camera functions of PollMode, MiddleMass, and ColorMode. The PollMode function is a

necessity because it makes the camera send the packets one at a time. Ideally, a consistent

stream of data would work better for detecting the position of an object, which would be 17

frames per second (fps) at the 112,500 baud rate used in the design, but the streaming data would

be difficult to parse and use correctly. Setting the MiddleMass and ColorMode are done to

specify the type of packets sent and to turn the cameras white balance on. The white balance is a

color filter for improved image quality with present IR light.

- 22 -

The FindColor state serves as a means to identify if a color is present. It does a “round

robin” check of each color until one of them is identified and then passes the color value on the

next state, TrackingState. In the TrackingState most of the information gathering is done on the

object being tracked by the robot. Average values are taking of the x position, y position, x

middle mass, y middle mass, confidence (tells the confidence of the objects presence), and pixels

(size in pixels of the object). Three packets of information have proven to be the optimal setting

for taking the average values. If the average confidence is greater than a threshold value of 20,

255 is the highest value, then the ViCoN-Bot goes into a new state, DataControl, or it is just sent

back to the FindColor state.

DataControl is where all the computation of the objects presence is made based on the

collected information. The servo direction is calculated from the middle mass values returned.

This value gives the location of the highest confidence for the center of the object being tracked.

At the time of this writing this produces sufficient results on the direction to send the ViCoN

servos but it will be necessary for a more robust direction calculation which will be discussed

further in the testing section. Similarly, the speed setting is calculated with the pixel size and

confidence but the speed of the robot is too great. Thus, the only usable speed is at the slowest

setting because the damage done to the robot will be “unimaginable” at higher speed if there is a

crash while testing. Figure 13 shows how the robot makes actual decisions based on the

situation and the color of the object being tracked. A in Figure 13 is a typical situation where

the robot would set the direction according to the data. B shows an example of how the camera

would return to the FindColor state since pink is in the line of sight of the robot even though

green is present. Pink serves as the dominant color and its presence supersedes the tracking of

any color. C shows the camera tracking a yellow object.

- 23 -

Figure 13. Shows how the ViCoN-Bot sets the direction and speed of the servos.

There are also a Reset and Idle state that are initialized through the pushbuttons located

on the Nios board. In theory, this is not a true state machine but more of top level control

program to initiate and communicate with all the lower levels of code in the design. A major

advantage to the program is the scalability it possess since additional colors to be tracked can be

easily added (pink, green, and yellow were the original colors used). But the functioning state

machine was a milestone in the ViCoN-Bot development. It was a milestone that unexpectedly

took longer than planned because of a calculation error within the parsing of the incoming

packets from the CMUcam. There was a delay that was made too short and the packets were not

being properly received. With a baud rate set to 112,500 it is necessary to give the camera at

least 400 ms to send a packet, because of the amount of characters returned, but when the delay

is too short only a portion of the information is collected.

Testing

The testing of the ViCoN-Bot was the last phase of the project development. At the time

of this paper 70-80% of the testing had been completed and the robot was fully functional. The

testing had shown that the robot could navigate through a designed course of objects at set sizes

and known distances. The lighting proved to also dramatically alter the results because the color

readings can change to such a high extent. There would have to be a physical body there at all

- 24 -

times to keep the ViCoN-Bot on the course though if an object was out of its line of sight. Most

of the testing was conducted in the College of Computing but ideally the robot will have much

more success in the Van Leer because of the improved lighting and most of the original

calculations were specified for that building. New calculations for the colors in the COC were

compiled but further testing will be needed for optimization in both buildings.

Originally the testing course was optimized for the layout of the Van Leer. Figure 14

shows the layout of the proposed course within the building (second floor).

Figure 14. Layout of the course to test the robot on the Van Leer second floor.

Placed at each corner, would be a green poster board signaling when to initiate a left turn to the

robot. The pink markers are hung in the middle of the hall, slightly higher than the ViCoN.

Regular yarn was used to hold the sign up with regular pushpins. A similar set up was used in

the COC by sectioning off one part of the hall and creating a smaller version of the course since

- 25 -

the hall dimensions are smaller there. Appendix D shows photographs of the track during the

actual testing of the robot, see Figure D-1 and Figure D-2, in the COC.

The testing portion is an ongoing process and as previously mentioned is at roughly 75%.

Many unexpected circumstances and new information about the functionality of the ViCoN has

been unveiled through this process. The most important is the effect of shadows. In areas of bad

illumination, such as the COC, the RGB values of a color can be drastically different. Figure 15

demonstrates how the color range of an object varies with overhead lighting. This effect limits

the robustness of the tracking since color ranges must be expanded to properly track a color.

Figure 15. Example in the color range difference depending on available overhead lighting.

The next phase of testing will focus on creating a more diverse way of calculating the

direction that produces better results in all lighting conditions. This should lead to a better

functionality while the robot is on the course. Also, to improve on the left turns when green is

the tracking color. Incorrect pixel (object size) values can through off the robots turn radius

making it too short or long. This will need further investigation because a small amount of times

during test runs of the course the ViCoN-Bot would start turning slightly early.

- 26 -

Organization

The ViCoN Design Project took place during the course of eleven weeks. Kwabena

Bosompem, Jeff Vickers, Kevin Walker, and Andre Moore are the four members of the

ViCoN team. There were two major milestones that had to be completed in order to

complete the project within the allotted time. Completely interfacing the Nios Board and the

CMUcam was the first milestone. This was to be completed by the end of week five. An

operational robot was the second milestone that should have been done by the middle of

week eight in order to finish the project. The Ghant chart in Figure 16 illustrates the major

phases that make up the ViCoN Project. It also shows the duration of each phase with

respect to the others.

Figure 16. Ghant chart outlining ViCoN design structure.

Research was the first major phase. This took place during the first two weeks of the

project. During this time the project team searched for digital cameras and considered what

programming device to use. When the digital camera was obtained and the programming

- 27 -

device was determined Jeff and Kevin tested the camera’s performance functionality, and

limitations. Kwabena and Andre completed the Nios Tutorial.

The second phase involved Kwabena and Andre working to swiftly interface the

CMUcam and the Nios Board. This involved sending commands to the camera via the Nios

Board, receiving feedback from the camera, and printing it to the LCD. Kevin and Jeff

designed a track and optimized the color ranges to be tracked.

Jeff and Kevin also worked on preparing the M1025 Hummer to interact with the

CMUcam and the Nios Board, which was the bulk of the third phase. This phase included

robot construction and installing power supplies. They also began designing a state-machine

to control the motions of the robot. Andre and Kwabena were completing the software code

that would interface all components of the ViCoN-project

Phase four dealt with the dealt with the designing and implementing pulse width

modulation. Jeff wrote the VHDL code to create pulse modulation for the direction and

speed servos in the ViCoN-Bot. Andre and Kwabena worked on integrating the VHDL code

in the top level design. Kevin began implementing the state machine into the source code.

At the end of this phase the team achieved pulse width modulation and was able to control

the servos. The robot was not fully operation by the end of week seven as planned but it was

full operational by the middle of week eight.

Testing and tweaking was the major component of phase five as well as completing

the project report, demonstration, and final presentation. Kevin completed the state-machine.

All four team members worked to tweak and test the ViCoN-Bot on the track. The team

made the final presentation and made a final demonstration.

Conclusion

Designing an autonomous, visually controlled robot, the ViCoN-Bot, encompassed nine

weeks from concept to functional prototype. The goal of the ViCoN project was simply to create

a vehicle that used a digital camera as a means of guidance around a course. The final prototype

used an FPGA, a digital camera, and two motors mounted in a miniature Hummer.

Design and implementation of the ViCoN-Bot was modular, providing a basic framework

for future prototypes to build upon. ViCoN’s modularity aids in a simplistic description of our

accomplishments. The first major milestone was the successful communication between the

- 28 -

FPGA and the camera. The second major milestone was the creation of a custom processor for

the FPGA that incorporates modules necessary to generate signals to control the motors. The

third and final major milestone was integrating the previous two and realizing the overall project.

The final phase involved positioning of brightly colored neon pink and green landmarks

in a fairly well lit hallway much like traffic lights. Objects were placed a reasonable distance

apart and elevated such that the ViCoN-Bot could pass underneath the object. The demo version

was limited to the detection and tracking of two color ranges, fluorescent green and pink. Neon

pink acted as a waypoint to seek and denoted forward movement. Similarly, neon green

signified a left hand turn.

The CMUcam’s inherent limitations included a 25° field of view, a low resolution of

80x143, and a low refresh rate of 17 frames per second affecting the ViCoN-Bot’s overall speed

and maneuverability. Moreover, the CMUcam’s imaging functions and algorithms were of an

extremely basic nature limiting the degree to which further processing could be applied. The

vehicle’s inability to spin or rotate about a point also limited the ability of the ViCoN-Bot to

locate a direction or speed indicator.

The following are a few suggestions for continuing versions of the ViCoN-Bot or for

groups interested in borrowing ViCoN modules. Altera has slated a new Nios board with a

processor approaching three times the speed of the current one, allowing for more complex and

speedy computations. Seattle Robotics and Carnegie Melon are also working on the next

CMUcam which offers much higher resolution and speeds. Adding a pan and tilt unit to the

camera will also help in finding objects.

Despite several problems encountered and subsequently surmounted we deem the

creation of the ViCoN-Bot a great exercise in team work, software design, hardware design and

prototyping of digital systems. The first prototype of the ViCoN-Bot successfully demonstrated

the control of a robot with a digital camera, just one of the endless possibilities of this vision

sensor.

A - 29

Appendix A:

CMUcam Data, Images, & User Manual

A - 30

Table A-1. Experimental tracking ranges for neon pink.

Color Min Max 3 meters Neon Pink Poster Red 240 240 Green 16 16 Blue 14 20 6 meters Neon Pink Poster

Red 240 240 Green 15 60 Blue 16 38 9 meters Neon Pink Poster Red 240 240 Green 16 22 Blue 16 21 12 meters Neon Pink Poster Red 230 240 Green 16 22 Blue 12 25

Track Neon Pink Poster

Red 220 240 Green 10 60 Blue 10 40

Table A-2. Experimental tracking ranges for neon green.

Color Min Max 3 meters Neon Green Poster

Red 98 120 Green 220 240 Blue 80 102 6 meters Neon Green Poster Red 85 110 Green 180 215 Blue 50 75 9 meters Neon Green Poster Red 75 90 Green 150 165 Blue 40 80 12 meters Neon Green Poster

Red 76 92 Green 140 160 Blue 40 100

Track Neon Green Poster Red 75 120 Green 140 240 Blue 40 100

A - 31

Table A-3. Experimental tracking ranges for neon yellow.

Color Min Max 3 meters Neon Yellow Poster Red 240 240 Green 240 240 Blue 55 65 6 meters Neon Yellow Poster Red 240 240 Green 240 240 Blue 45 70 9 meters Neon Yellow Poster Red 240 240 Green 240 240 Blue 40 80 12 meters Neon Yellow Poster Red N/A N/A Green N/A N/A Blue N/A N/A

Track Neon Yellow Poster Red 235 240 Green 235 240 Blue 40 80

Figure A-1. Tracking neon pink and green posters in EE hallway at 3, 6, 9, 12, and 15 meters.

A - 32

Table A-4. Size and confidence measurements of a green poster at close proximity.

1 meters Neon Green Poster 1.5 meters Neon Green Poster Size 255 Size 189 Confidence 238 Confidence 240 2 meters Neon Green Poster 2.5 meters Neon Green Poster Size 125 Size 85 Confidence 235 Confidence 228 3 meters Neon Green Poster 3.5 meters Neon Green Poster Size 64 Size 50 Confidence 230 Confidence 221 4 meters Neon Green Poster 4.5 meters Neon Green Poster Size 40 Size 30 Confidence 236 Confidence 230 5 meters Neon Green Poster 5.5 meters Neon Green Poster Size 23 Size 19 Confidence 229 Confidence 202

B - 33

Appendix B:

Nios Board Interface Diagrams & C Code

1 #include "nios.h"23 // Universal Definitions4 #define SUCCESS 15 #define FAILURE 06 #define YES 17 #define NO 08 #define TRUE 19 #define FALSE 0

1011 /*-----------SPEEDS-------------*/12 #define BRAKE 013 #define STOP 014 #define FULL_FORWARD 715 #define HALF_FORWARD 616 #define QUARTER_FORWARD 517 #define EIGHTH_FORWARD 418 #define NEUTRAL 319 #define QUARTER_REVERSE 220 #define EIGHTH_REVERSE 1212223 /*-----------DIRECTIONS-------------*/24 #define STOP 0 //??25 #define LEFT_10 126 #define LEFT_20 227 #define LEFT_45 328 #define LEFT_0 429 #define STRAIGHT 430 #define RIGHT_0 431 #define RIGHT_10 532 #define RIGHT_20 633 #define RIGHT_45 7343536 //Interrupt Context (what caused it)37 typedef struct{ // Struct to hold pointer to uart struct38 np_uart *uart; // Pointer to uart struct see nios.h39 char rxChar; // Last byte sent through UART see nios.h40 } UARTISRContext; // Named such to .... context of interrupt41424344 typedef struct{45 int rmin;46 int rmax;47 int gmin;48 int gmax;49 int bmin;50 int bmax;51 }Color;5253 static Color PINK = {210, 240, 0, 60, 10, 30};54 static Color YELLOW = {225, 240, 225, 240, 30, 80};55 static Color GREEN = {65, 120, 130, 240, 30, 100};5657 //Type C packet58 typedef struct{59 int x1; //The left most corner x value60 int y1; //The left most corners y value61 int x2; //The right most corners x value62 int y2; //The right most corners y value63 int pixels; //# of Pixels in the tracked region,64 int confidence; //The (# of pixels / area)*256 of the65 // bounded rectangle and capped at 25566 }CdataPacket;67

6869 //Type M packet70 typedef struct{71 int mx; //The middle of mass x value72 int my; //The middle of mass y value73 int x1; //The left most corners x value74 int y1; //The left most corners y value75 int x2; //The right most corners x value76 int y2; //The right most corners y value77 int pixels; //# of Pixels in the tracked region78 int confidence; //The (# of pixels / area)*256 of the79 // bounded rectangle and capped at 25580 }MdataPacket;818283 //Type N packet84 typedef struct{85 int spos; //The current servo position86 int mx; //The middle of mass x value87 int my; //The middle of mass y value88 int x1; //The left most corners x value89 int y1; //The left most corners y value90 int x2; //The right most corners x value91 int y2; //The right most corners y value92 int pixels; //# of Pixels in the tracked region93 int confidence; //The (# of pixels / area)*256 of the94 // bounded rectangle and capped at 25595 }NdataPacket;969798 //Type S packet99 typedef struct{

100 int rmean ; //the mean Red or Cr (approximates r-g)101 int gmean ; //the mean Green or Y (approximates intensity)102 int bmean ; //the mean Blue or Cb (approximates b-g)103 int rdev ; //the *deviation of red or Cr104 int gdev ; //the *deviation of green or Y105 int bdev ; //the *deviation of blue or Cb106 }SdataPacket;107108109 /*----------FUNCTIONS PROTOTYPES--------------*/110111 // Vicon_Camera.c112 int SendCommand(char *s);113 int DisplayResponse(void);114 int EmptyArray(void);115 void ViconMagic(char *s);116 void ViconMagic2(char *s);117 int ViconTest(void);118 int PacketTokenizer(void);119 int UpdatePackets(void);120 int SetCameraIdle(void);121 int SetContrast (int contrastI);122 int GetContrast(void);123 int SetBrightness (int brightnessI);124 int GetBrightness(void);125 int SetColorMode (int colormode);126 int GetColorMode(void);127 int SetClockSpeed(int clockspeedI);128 int GetClockSpeed(void);129 int SetExposure(int exposureI);130 int GetExposure(void);131 int ToggleExposure(void);132 int DumpFrame(void);133 int SetDelay(int delayI);134 int GetDelay(void);

135 int GetMeanColor(void);136 int GetVersion ( void );137 int SetHalfHorizontalResolutionMode(int active);138 int GetHalfHorizontalResolutionMode(void);139 int GetServoInput(void);140 int SetTrackingLight(int trackingI);141 int SetLineMode(void);142 int SetMiddleMass(int middlemassI);143 int SetNoiseFilter(int noisefilterI);144 int ToggleNoiseFilter(void);145 int GetNoiseFilter (void);146 int SetPollMode(int pollmodeI);147 int GetPollMode(void);148 int SetRawMode(int rawmodeI);149 int GetRawMode (void);150 int ResetCamera(void);151 int SetServoPosition(int positionI);152 int SetSwitchingMode(int switchingmodeI);153 int GetSwitchingMode(void);154 int SetWindowSize(int x, int y, int x1, int y1);155 int TrackWindow(void);156 int TrackColor(Color c);157 void InterruptServiceRoutine(int context);158 void DisableUartInterrupt(void);159 void EnableUartInterrupt(void);160 void InterruptHandler (int context);161162 // Vicon_Servo.c163 int GetSpeed(void);164 int GetDirection(void);165 void SetSpeed(int spd);166 void SetDirection(int drx);167168 // Vicon.c169 int main2(void);170 int FindColor(void);171 int CameraStart(void);172 int TrackState(void);173 int DataControl(int con, int m1, int m2, int pix);174 //int SendServo(ServoValue);175 int Reset(void);176 void MyPIO_ISR(int context);177 void InitializeCamera(void);178179 // Vicon_pacet_test.c180 int CheckPackets(void);181182

1 #include "vicon.h"2 #include "nios.h"3 #include "pio_lcd16207.h"4 #include <stdio.h>5 #include <string.h>67 /*-----------------------------------------------------------------8 * Author(s): Kwabena Asare Bosompem9 * Andre Moore

10 * Jeff Vickers11 * Kevin Walker12 * Created: May 30th, 2002.13 * Modified: June 15th, 2002.14 * Description: Functions for Camera Control15 * Version: 0.1a16 * Comments: Every function that returns a data packet assumes17 * the poll mode is set to 1. Default camera setting18 * is 0. Also no raw mode. Not necessary for Nios.19 *-----------------------------------------------------------------*/20212223 /*-----------------------------------------------------------------*24 * Definitions & Global Variables25 *-----------------------------------------------------------------*/26 char lastresponse[100];27 const char VER[18] = "ACK CMUcam v1.12 :";28 const char RVER[19] = "ACK CMUcam v1.12 :";29 const char ACK[5] = "ACK :";30 const char NCK[5] = "NCK :";31 const char COL[2] = ": ";32 const char VER_RM[14] = "CMUcam v1.12 :";33 const char RVER_RM[15] = " CMUcam v1.12 :";34 const char ACK_RM[1] = ":";353637 const int AUTOGAIN_ON = 33; //DEFAULT38 const int AUTOGAIN_OFF = 32;3940 const int YCC_ON = 36;41 const int YCC_OFF = 32;42 const int RGB_ON = 44;43 const int RGB_OFF = 40;4445 const int TRACKING_ON = 1;46 const int TRACKING_OFF = 0;47 const int TRACKING_AUTO = 2;4849 int crindex = 0; // Keep track of camera response array50 UARTISRContext gC = {0,-1}; // Context and last byte preset.51 int k=0; // Index of array to be tokenized for52 // packets;53 int crtemp=0; // Temporary int holds54 int temp=0; // Temporary int holds5556 SdataPacket spacket ={0};57 MdataPacket mpacket ={0};58 NdataPacket npacket ={0};59 CdataPacket cpacket ={0};606162 int pollmode = 0; // Initialize poll mode63 int rawmode = 2; // Intialize raw mode64 int mmassmode = 1; // Initialize middle mass mode65 int uartstatus = 0; // Status of serial port66 int clockspeed = 17; // Default values67 int contrast = 127;

68 int brightness = 127;69 int exposure = 1;70 int delay = 1;71 int colormode = 40;72 int active = 0;73 int tracking = 2;74 int middlemass = 1;75 int noisefilter = 1;76 int position = 0;77 int switchingmode = 0;7879 /*-----------------------------------------------------------------*80 * F U N C T I O N S81 *-----------------------------------------------------------------*/8283 /**84 * S E N D C O M M A N D ( S T R I N G )85 * Description: Sends through the uart(serial port) a preformatted86 * command(string) to the CMUcam.87 * Created: June 6th, 2002.88 * Paramaters: Pointer to a string(s)89 * Returns: Int indicating success or failure90 * Comments: See nios_peripheral_reference_manual.pdf for91 * details on nr_uart_txstring92 * Modified: June 14th, 2002.93 */94 int SendCommand(char *s)95 {96 uartstatus = nr_uart_txstring(s);97 if (uartstatus > 0)98 return SUCCESS;99 else

100 return FAILURE;101102 }//end of SendCommand(char*)103104105106 /**107 * D I S P L A Y R E S P O N S E ()108 * Description: Displays to the LCD the last string recieved from109 * the CMUcam.110 * Created: June 6th, 2002.111 * Paramaters: Void112 * Returns: int to indicate successful print to lcd113 * Comments: Took delay out. Delays need to be added in main.114 * Modified: June 15th, 2002.115 */116 int DisplayResponse(void)117 {118 nr_pio_lcdwritescreen(lastresponse);119 return SUCCESS;120121 }//end of Display Response122123124125 /**126 * E M P T Y A R R A Y ( )127 * Description: Empties the array holding a string of the last128 * camera response129 * Created: June 6th, 2002.130 * Paramaters: Void131 * Returns: int132 * Comments: Currently looking for quicker way to achieve this133 * with pointers134 * Modified: June 14th, 2002.

135 */136 int EmptyArray()137 {138 int i;139 for (i=0; i<100;i++)140 {141 lastresponse[i]= 0; // 0 is ascii for null142 }143 return SUCCESS;144145 }// end of EmptyArray()146147148149 /**150 * V I C O N M A G I C ( S T R I N G )151 * Description: Go Figure. It calls 4 functions152 * Created: June 15th, 2002.153 * Paramaters: s. a character array of command to send camera154 * Returns: Nothing155 * Comments: None156 * Modified: June 15th, 2002.157 */158 void ViconMagic(char *s)159 {160 crindex = 0;161 k = 0;162 EmptyArray();163 EnableUartInterrupt();164 SendCommand(s);165 nr_delay(100);166 DisableUartInterrupt();167168 }//end of ViconMagic169170 void ViconMagic2(char *s)171 //The packets need extra time in a delay since it is more characters returned.172 //Without the 1/2 ms delay the packets would be clipped.173 {174 crindex = 0;175 k = 0;176 EmptyArray();177 EnableUartInterrupt();178 SendCommand(s);179 nr_delay(500);180 DisableUartInterrupt();181182 }//end of ViconMagic183184 /**185 * V I C O N T E S T ( V O I D )186 * Description: Go Figure.187 * Created: June 15th, 2002.188 * Paramaters: void189 * Returns: int SUCCESS or FAILURE190 * Comments: None191 * Modified: June 15th, 2002.192 */193 int ViconTest(void)194 {195 int rmode;196 rmode = GetRawMode();197 rmode = rmode & 2;198 if (rmode == 2)199 if (strcmp(ACK,lastresponse)!=0)200 return FAILURE;201 else

202 return SUCCESS;203 else204 if (strcmp(ACK_RM,lastresponse)!=0)205 return FAILURE;206 else207 return SUCCESS;208209 }//end of ViconTest210211212213 /**214 * P A C K E T T O K E N I Z E R215 * Description: Dumps bitmap of current still image216 * Created: June 13th, 2002.217 * Paramaters: Void218 * Returns: Int219 * Comments: Splits into consistuent numbers220 * Modified: June 7th, 2002.221 */222 int PacketTokenizer(void){223 int xi;224 while(lastresponse[++k]== 32);225 crtemp =lastresponse[k];226 while((crtemp<58) && (crtemp>47)){227 temp = temp*10 + crtemp - 48;228 k++;229 crtemp =lastresponse[k];230 }//end of231 xi = temp;232 temp = 0;233 return xi;234235 }//end of PacketTokenizer()236237238239 /**240 * U P D A T E P A C K E T S ( V O I D )241 * Description: Dumps bitmap of current still image242 * Created: June 13th, 2002.243 * Paramaters: Void244 * Returns: Int245 * Comments: Splits into consistuent numbers246 * Modified: June 7th, 2002.247 */248 int UpdatePackets(void){249250 while((k<100) || (lastresponse[k] == 0)){251 if(lastresponse[k] == 65 ) // 65 is ascii A252 {253 k++;254 k++;255 }256 if(lastresponse[k] == 83 ) // 83 is ascii S257 {258 spacket.rmean = PacketTokenizer();259 spacket.gmean = PacketTokenizer();260 spacket.bmean = PacketTokenizer();261 spacket.rdev = PacketTokenizer();262 spacket.gdev = PacketTokenizer();263 spacket.bdev = PacketTokenizer();264 }265 if(lastresponse[k] == 67 ) // 67 is ascii C266 {267 cpacket.x1 = PacketTokenizer();268 cpacket.y1 = PacketTokenizer();

269 cpacket.x2 = PacketTokenizer();270 cpacket.y2 = PacketTokenizer();271 cpacket.pixels = PacketTokenizer();272 cpacket.confidence = PacketTokenizer();273 }274 if(lastresponse[k] == 77 ) // 77 is ascii M275 {276 mpacket.mx = PacketTokenizer();277 mpacket.my = PacketTokenizer();278 mpacket.x1 = PacketTokenizer();279 mpacket.y1 = PacketTokenizer();280 mpacket.x2 = PacketTokenizer();281 mpacket.y2 = PacketTokenizer();282 mpacket.pixels = PacketTokenizer();283 mpacket.confidence = PacketTokenizer();284 }285 if(lastresponse[k] == 78 ) // 78 is ascii N286 {287 npacket.mx = PacketTokenizer();288 npacket.mx = PacketTokenizer();289 npacket.my = PacketTokenizer();290 npacket.x1 = PacketTokenizer();291 npacket.y1 = PacketTokenizer();292 npacket.x2 = PacketTokenizer();293 npacket.y2 = PacketTokenizer();294 npacket.pixels = PacketTokenizer();295 npacket.confidence = PacketTokenizer();296 }297 k++;298 }299 return SUCCESS;300301 }//end of UpdatePackets302303304305 /************************************************************/306 /**************************[ pg.13 ]*************************/307 /************************************************************/308309 /**310 * S E T C A M E R A I D L E ( )311 * Description: Sets Camera Idle. Stops streaming of packets in312 * when camera is in tracking mode.313 * Created: June 7th, 2002.314 * Paramaters: Void315 * Returns: Int SUCCESS or FAILURE316 * Comments:317 * Modified: June 15th, 2002318 */319 int SetCameraIdle(void)320 {321 char cmdtosend [4] = "\r";322 ViconMagic(cmdtosend);323 return ViconTest();324325 }//end of SetCameraIdle()326327328329 /**330 * S E T C O N T R A S T ( I N T )331 * Description: Set's the camera's contrast332 * Created: June 15th, 2002.333 * Paramaters: contrastI. an int from 0-255334 * Returns: int (SUCCESS or FAILURE)335 * Comments:

336 * Modified: June 15th, 2002.337 */338 int SetContrast (int contrastI)339 {340 char cmdtosend [6];341 contrast = contrastI;342 if((contrast >255) || (contrast < 0))343 return FAILURE ;344 sprintf(cmdtosend, "CR 5 %d\r", contrast);345 ViconMagic(cmdtosend);346 return ViconTest();347348 }//end of SetContrast(int)349 int GetContrast(void)350 {351 return contrast;352353 }//end of GetExposure()354355356357 /**358 * S E T B R I G H T N E S S ( I N T )359 * Description: Set's the camera's brightness360 * Created: June 15th, 2002.361 * Paramaters: brightnessI. an int from 0-255362 * Returns: int (SUCCESS or FAILURE)363 * Comments:364 * Modified: June 15th, 2002.365 */366 int SetBrightness (int brightnessI)367 {368 char cmdtosend [6];369 brightness = brightnessI;370 if((brightness >255) || (brightness < 0))371 return FAILURE ;372 sprintf(cmdtosend, "CR 6 %d\r", brightness);373 ViconMagic(cmdtosend);374 return ViconTest();375376 }//end of SetBrightness(int)377 int GetBrightness(void)378 {379 return brightness;380381 }//end of GetExposure()382383384385 /**386 * S E T C O L O R M O D E ( I N T )387 * Description: Set's the camera's color mode388 * Created: June 15th, 2002.389 * Paramaters: colormode. see vicon.h390 * Returns: int (SUCCESS or FAILURE)391 * Comments: See CMUcam manual for command392 * CR393 * Modified: June 15th, 2002.394 */395 int SetColorMode (int colormode)396 {397 char cmdtosend [6];398 if((colormode >44) || (colormode < 32)) //inadequate check399 return FAILURE ;400 sprintf(cmdtosend, "CR 18 %d\r", colormode);401 ViconMagic(cmdtosend);402 return ViconTest();

403404 }//end of SetContrast(int)405 int GetColorMode(void)406 {407 return colormode;408409 }//end of GetExposure()410411412413 /**414 * S E T C L O C K S P E E D ( I N T )415 * Description: Set's camera's speed in fps416 * Created: June 15th, 2002.417 * Paramaters: clockpseedI The number of frames/s418 * Returns: int (SUCCESS or FAILURE)419 * Comments:420 * Modified: June 15th, 2002.421 */422 int SetClockSpeed(int clockspeedI)423 {424 char cmdtosend [6];425 if((clockspeedI>12) || (clockspeedI< 2)) //inadequate check426 return FAILURE ;427 clockspeed = clockspeedI;428 sprintf(cmdtosend, "CR 17 %d\r", colormode);429 ViconMagic(cmdtosend);430 return ViconTest();431432 }//end of SetClockSpeed(int)433 int GetClockSpeed(void)434 {435 return clockspeed;436437 }//end of GetExposure()438439440441 /**442 * S E T E X P O S U R E()443 * Description: Don't quite understand what it does for the camera444 * Created: June 15th, 2002.445 * Paramaters: int AUTOGAIN_ON or AUTOGAIN_OFF446 * Returns: int (SUCCESS or FAILURE)447 * Comments:448 * Modified: June 15th, 2002.449 */450 int SetExposure(int exposureI)451 {452 char cmdtosend [6];453454 if(exposureI != AUTOGAIN_ON || exposureI != AUTOGAIN_OFF)455 return FAILURE ;456457 exposure = exposureI;458 sprintf(cmdtosend, "CR 19 %d\r", exposure);459460 ViconMagic(cmdtosend);461 return ViconTest();462463 }//end of SetExposure464 int GetExposure(void)465 {466 return exposure;467468 }//end of GetExposure()469 int ToggleExposure(void)

470 {471 int status;472 if(exposure == AUTOGAIN_ON)473 status = SetExposure(AUTOGAIN_OFF);474 else475 status = SetExposure(AUTOGAIN_ON);476 return status;477 }//end of ToggleExposure478479480481 /************************************************************/482 /**************************[ pg.14 ]*************************/483 /************************************************************/484485 /**486 * D U M P F R A M E ( )487 * Description: Dumps bitmap of current still image488 * Created: June 7th, 2002.489 * Paramaters: Void490 * Returns: Int491 * Comments: Not to be used. Not beneficial to ViCoN-Bot492 * Transmission time for 80*143493 * Memory required to hold 80*143 extra bytes494 * Processing power and extra memory required to manipulate it495 * Modified: June 15th, 2002.496 */497 int DumpFrame(void)498 {499 char command [5] = "DF\r";500 //Not to be used501 //ViconMagic(command);502 nr_pio_lcdwritescreen("Gee! what were you thinking");503 nr_delay(100000); //10 Sec wait. Same time it takes to dump frame504 return SUCCESS;505506 }//end of DumpFrame507508509510 /**511 * S E T D E L A Y ( I N T )512 * Description: Sets delay before camera transmits packets513 * Created: June 15th, 2002.514 * Paramaters: delayI 0-255 1 unit = baud rate515 * Returns: int (SUCCESS or FAILURE )516 * Comments: see CMUcam Manual for details517 * We used firmware v1.12518 *519 * Modified: June 15th, 2002.520 */521 int SetDelay(int delayI)522 {523 char cmdtosend [6];524 if((delayI>255) || (delayI< 0)) //inadequate check525 return FAILURE ;526 delay = delayI;527 sprintf(cmdtosend, "DM %d\r", delay);528 ViconMagic(cmdtosend);529 return ViconTest();530531 }//end of SetDelay532 int GetDelay(void)533 {534 return delay;535536 }//end of GetDelay

537538539540 /**541 * G E T M E A N C O L O R ( )542 * Description: Get mean color543 * Created: June 15th, 2002.544 * Paramaters:545 * Returns: spacket with most current measurements546 * Comments: In streaming mode it will return the last spacket and547 * fill unknown values with zero if it is truncated.548 * Modified: June 15th, 2002.549 */550 int GetMeanColor(void)551 {552 char cmdtosend [4] = "GM\r";553 ViconMagic(cmdtosend);554 UpdatePackets();555 return SUCCESS;556557 }//end of GetMeanColor558559560561 /************************************************************/562 /**************************[ pg.15 ]*************************/563 /************************************************************/564 /**565 * G E T V E R S I O N ( )566 * Description: Gets current Camera Version567 * Created: June 7th, 2002.568 * Paramaters: Void569 * Returns: Int (SUCCESS or FAILURE)570 * Comments:571 * Modified: June 15th, 2002.572 */573 int GetVersion ( void )574 {575 char command [5] = "GV\r";576 int rmode;577578 ViconMagic(command);579 rmode = GetRawMode();580 rmode = rmode & 2;581 if (rmode = 2)582 if (strcmp(VER,lastresponse)!=0)583 return FAILURE;584 else585 return SUCCESS;586 else587 if (strcmp(VER_RM,lastresponse)!=0)588 return FAILURE;589 else590 return SUCCESS;591592 }//end of GetVersion()593594595596 /**597 * SET HALF HORIZONTAL RESOLUTION (INT)598 * Description: Puts the camera into half-horizontal resolution mode599 * for DumpFrame and SetLineMode600 * Created: June 15th, 2002.601 * Paramaters:602 * Returns: int (SUCCESS or FAILURE )603 * Comments: Not to be implemented. Irrelevant in ViCON-Bot operation

604 *605 *606 * Modified: June 15th, 2002.607 */608 int SetHalfHorizontalResolutionMode(int active)609 {610 //active = 1 => every odd column processed611 //active = 0 => disable this mode612 nr_pio_lcdwritescreen("HM ? Why oh why");613 nr_delay(100000); //Wait 10 secs614 return SUCCESS;615616 }//end of SetHalfHorizontalResolution()617 int GetHalfHorizontalResolutionMode(void)618 {619 return active;620621 }//end of SetHalfHorizontalResolution()622623624625 /**626 * GET SERVO INPUT627 * Description: Gets the voltage on the servo line.628 * 0 = line low. 1 = line high629 * Created: June 15th, 2002.630 * Paramaters: None631 * Returns: int -1 failure (0 low voltage or 1 high voltage)632 * Comments:633 * Modified: June 15th, 2002.634 */635 int GetServoInput(void)636 {637 char cmdtosend [4] = "I1\r";638 int pos;639 int rmode;640641 ViconMagic(cmdtosend);642643 rmode = GetRawMode();644 rmode = rmode & 2;645646 if (rmode = 2)647 pos = 0;648 else649 pos = 4;650651 if(lastresponse[pos] == 48)652 return 0;653 else if(lastresponse[pos] == 49)654 return 1;655 else656 return -1;657658 }//end of GetServoInput659660661662 /**663 * S E T T R A C K I N G L I G H T ( I N T )664 * Description: Controls the tracking light.665 * Acceptable values 0,1,2(default)666 * Created: June 15th, 2002.667 * Paramaters: trackingI668 * Returns: int (SUCCESS or FAILURE )669 * Comments:670 * Modified: June 15th, 2002.

671 */672 int SetTrackingLight(int trackingI)673 {674 char cmdtosend [6];675676 if((trackingI >2) || (trackingI < 0))677 return FAILURE ;678679 tracking = trackingI;680 sprintf(cmdtosend, "L1 %d\r", tracking);681682 ViconMagic(cmdtosend);683 return ViconTest();684685 }//end of SetTrackingLight686687688689 /************************************************************/690 /**************************[ pg.16 ]*************************/691 /************************************************************/692693 /**694 * S E T L I N E M O D E ( I N T )695 * Description: Turns on line mode696 * Created: June 15th, 2002.697 * Paramaters:698 * Returns: int (SUCCESS or FAILURE )699 * Comments: Will not be implemented700 * Over complicates the code for the701 * ViCoN-Bot702 * Modified: June 15th, 2002.703 */704 int SetLineMode(void)705 {706 return SUCCESS;707708 }//end of SetLineMode();709710711712 /************************************************************/713 /**************************[ pg.17 ]*************************/714 /************************************************************/715 /**716 * S E T M I D D L E M A S S ( I N T )717 * Description: Set middle mass718 *719 * Created: June 15th, 2002.720 * Paramaters:721 * Returns: int (SUCCESS or FAILURE )722 * Comments:723 *724 *725 * Modified: June 15th, 2002.726 */727 int SetMiddleMass(int middlemassI)728 {729 char cmdtosend [6];730731 //if((middlemassI >10) || (middlemassI < 0))732 // return FAILURE ;733734 middlemass = middlemassI;735 sprintf(cmdtosend, "MM %d\r", middlemass);736 ViconMagic(cmdtosend);737 return ViconTest();

738739 }//end of SetMiddleMass(int)740741742743 /**744 * S E T N O I S E F I L T E R ( I N T )745 * Description: Controls the Noise filter setting on the camera746 * 1- more selective tracking (default)747 * 0- less selective tracking748 * Created: June 15th, 2002.749 * Paramaters:750 * Returns: int (SUCCESS or FAILURE )751 * Comments:752 *753 *754 * Modified: June 15th, 2002.755 */756 int SetNoiseFilter(int noisefilterI)757 {758 char cmdtosend [6];759760 if((noisefilterI >2) || (noisefilterI < 0))761 return FAILURE ;762763 noisefilter = noisefilterI;764 sprintf(cmdtosend, "NF %d\r", noisefilter);765 ViconMagic(cmdtosend);766 return ViconTest();767 }//end of SetNoiseFilter768 int ToggleNoiseFilter(void)769 {770 if(noisefilter == 0)771 return SetNoiseFilter(1);772 else773 return SetNoiseFilter(0);774775 }//end of ToggleNoiseFilter776 int GetNoiseFilter (void)777 {778 return noisefilter;779 }//end of GetNoiseFilter780781782783 /**784 * S E T P O L L M O D E ( I N T )785 * Description: Switchs packet dumping from streaming to a line786 * at a time787 * Created: June 7th, 2002.788 * Paramaters: Int modeI (see CMUcam Manual)789 * Returns: Int790 * Comments: Very Functional791 *792 * Modified: June 7th, 2002.793 * Length: 24794 */795 int SetPollMode(int pollmodeI)796 {797 char cmdtosend [6];798 //if((pollmodeI == 0) || (pollmodeI == 1))799 // return FAILURE;800 pollmode = pollmodeI;801 sprintf(cmdtosend,"PM %d\r",pollmode);802 ViconMagic(cmdtosend);803 DisplayResponse();804 nr_delay(800);

805 return ViconTest();806807 }//end of SetPollMode(int)808 int GetPollMode(void)809 {810 return pollmode;811812 }//end of SetPollMode(int)813814815816 /************************************************************/817 /**************************[ pg.18 ]*************************/818 /************************************************************/819820 /**821 * S E T R A W M O D E822 * Description: Sets the input to or output from the camera823 *824 *825 * Created: June 11th, 2002.826 * Paramaters:827 * Returns:828 * Comments: Very Functional829 * Modified: June 15th, 2002.830 */831 int SetRawMode(int rawmodeI)832 {833 char cmdtosend [5];834 //if((rawmodeI == 0) || (rawmodeI == 2)) //avoids other modes835 // return FAILURE;836 rawmode = rawmodeI;837 sprintf(cmdtosend,"RM %d\r",rawmode);838 ViconMagic(cmdtosend);839 return ViconTest();840841 }//end of SetPollMode(int)842 int GetRawMode (void)843 {844 return rawmode;845846 }//end of GetRawMode847848849850 /**851 * R E S E T C A M E R A ( )852 * Description: Resets camera853 * Created: June 15th, 2002.854 * Paramaters: None855 * Returns: int (SUCCESS or FAILURE )856 * Comments:857 *858 * Modified: June 15th, 2002.859 */860 int ResetCamera(void)861 {862 char command [5] = "RS\r";863 int rmode;864865 ViconMagic(command);866 rmode = GetRawMode();867 rmode = rmode & 2;868 if (rmode = 2)869 if (strcmp(RVER,lastresponse)!=0)870 return FAILURE;871 else

872 return SUCCESS;873 else874 if (strcmp(RVER_RM,lastresponse)!=0)875 return FAILURE;876 else877 return SUCCESS;878879 }//end of ResetCamera880881882883 /**884 * S E T S E R V O P O S I T I O N ( I N T )885 * Description: Set's the servo position886 * Created: June 15th, 2002.887 * Paramaters: position 0-255888 * Returns: int (SUCCESS or FAILURE )889 * Comments:890 *891 *892 * Modified: June 15th, 2002.893 */894 int SetServoPosition(int positionI)895 {896 char cmdtosend [5];897 //if((positionI > 255) || (positionI < 0)) //avoids other modes898 // return FAILURE;899 position = positionI;900 sprintf(cmdtosend,"S1 %d\r",position);901 ViconMagic(cmdtosend);902 return ViconTest();903904 }//end of SetServoPosition()905906907908 /************************************************************/909 /**************************[ pg.19 ]*************************/910 /************************************************************/911912 /**913 * S E T S W I T C H I N G M O D E ( I N T )914 * Description: Switch between color and statistic packets915 * in tracking modes. 0- off 1-on916 * Created: June 15th, 2002.917 * Paramaters: switchingmodeI918 * Returns: int (SUCCESS or FAILURE )919 * Comments:920 *921 * Modified: June 15th, 2002.922 */923 int SetSwitchingMode(int switchingmodeI)924 {925 char cmdtosend [5];926 //if(switchingmodeI != 0 || switchingmodeI != 1) //avoids other modes927 // return FAILURE;928 switchingmode = switchingmodeI;929 sprintf(cmdtosend,"SM %d\r",switchingmode);930 ViconMagic(cmdtosend);931 return ViconTest();932933 }//end of SetSwitchingMode934 int GetSwitchingMode(void)935 {936 return switchingmode;937938 }//end of GetSwitchingMode

939940941942 /**943 * SET WINDOW SIZE (INT INT INT INT)944 * Description: Simply sets the window size of the camera945 * Created: June 15th, 2002.946 * Paramaters: 2 coordinates top-left bottom-right947 * Returns: int (SUCCESS or FAILURE )948 * Comments:949 *950 *951 * Modified: June 15th, 2002.952 */953 int SetWindowSize(int x, int y, int x1, int y1)954 {955 char cmdtosend [20];956 if((x>x1 || y>y1) || ((x<1 || y<1) || (y1>143 || x1>80)))957 return FAILURE;958959 sprintf(cmdtosend,"SW %d %d %d %d\r",x,y,x1,y1);960961 ViconMagic(cmdtosend);962 return ViconTest();963964 }//end of SetWindowSize965966967968 /************************************************************/969 /**************************[ pg.20 ]*************************/970 /************************************************************/971972 /**973 * T R A C K W I N D O W ( )974 * Description: Tracks prominent color in window975 * Created: June 5th, 2002.976 * Paramaters: Void977 * Returns: int978 * Comments: Not to be used. Not beneficial for ViCoN-Bot979 * Modified: June 15th, 2002.980 */981 int TrackWindow(void)982 {983 char cmdtosend [3] = "TW\r";984 ViconMagic(cmdtosend);985 return UpdatePackets();986987 }//end of TrackWindow()988989990991 /**992 * T R A C K C O L O R ( I N T )993 * Description: Tracks a given color994 * Created: June 5th, 2002.995 * Paramaters:996 * Returns:997 * Comments:998 * Modified: June 15th, 2002.999 */

1000 int TrackColor(Color c) //array of color values1001 {1002 char cmdtosend [20];1003 //need to check array size and valid values1004 sprintf(cmdtosend,"TC %d %d %d %d %d %d \r",c.rmin, c.rmax, c.gmin, c.gmax, c

.bmin, c.bmax);

1005 ViconMagic2(cmdtosend);1006 UpdatePackets();1007 //return UpdatePackets();10081009 }//end of TrackColor1010101110121013 /************************************************************/1014 /************************[ THE END ]*************************/1015 /************************************************************/1016 /**1017 * INTERRUPT SERVICE ROUTINE()1018 * Description: Handles all interrupt service routines1019 * Created: June 5th, 2002.1020 * Paramaters:1021 * Returns: int1022 * Comments: Not to be used. Not beneficial for ViCoN-Bot1023 * Modified: June 11th, 2002.1024 */1025 void InterruptServiceRoutine(int context)1026 {1027 UARTISRContext *c = (UARTISRContext *)context;1028 int status;1029 int rxChar;1030 //char joy;1031 status = c->uart->np_uartstatus;1032 rxChar = c->uart->np_uartrxdata;1033 c->uart->np_uartstatus = 0; // clear the interrupt condition10341035 if(status & np_uartstatus_rrdy_mask ){1036 if(rxChar == 13)1037 rxChar = 32;1038 if(crindex>=0)1039 lastresponse[crindex]= (char)rxChar;1040 //joy = (char)rxChar;1041 //lastresponse[crindex]= joy;1042 crindex++;1043 }1044 c->rxChar = rxChar; // save the character for later use.10451046 }//end of InterruptServiceRoutine1047104810491050 /**1051 * ENABLE UART INTERRUPT()1052 * Description: Enables Uart as an interrupt1053 * Created: June 5th, 2002.1054 * Paramaters: int1055 * Returns: void1056 * Comments:1057 * Modified: June 7th, 2002.1058 */1059 void EnableUartInterrupt(void)1060 {1061 gC.uart = (np_uart *)na_uart1;1062 nr_installuserisr(na_uart1_irq,InterruptServiceRoutine,(long)&gC);1063 {1064 np_uart *uart;1065 uart = na_uart1;1066 uart->np_uartcontrol = np_uartcontrol_irrdy_mask;10671068 }//end of thread. don't understand this10691070 }//end of EnableUartInterrupt()1071

107210731074 /**1075 * DISABLE UART INTERRUPT()1076 * Description: Disables Uart as an interrupt1077 * Created: June 5th, 2002.1078 * Paramaters: int1079 * Returns: void1080 * Comments:1081 * Modified: June 7th, 2002.1082 */1083 void DisableUartInterrupt(void)1084 {1085 np_uart *uart;1086 uart = na_uart1;1087 uart->np_uartcontrol = 0;1088 nr_installuserisr(na_uart1_irq,0,0);10891090 }//end of DisableUartInterrupt()109110921093

C - 54

Appendix C:

Pulse Width Modulation Calculations & Code

C - 55

Table C-1. Pulse width modulation calculations and settings.

Clock (kHz) Base Pulse (ms)

# of clock Cycles

104.20 1 104

Desired Speed Speed Setting #

Pulse Width Duty Cycle

Duty Cycle Error

3-bit Value

# of Cycles

N/A Stop / Brake 0 0 0% 0% 000 N/A - 1/4 Quarter Reverse 2 1.375 7.164% 0.025% 010 1843 - 1/8 Eighth Reverse 1 1.438 7.489% -0.013% 001 1850

0 Neutral 3 1.500 7.815% 0.008% 011 1856 1/8 Eighth Forward 4 1.563 8.141% 0.012% 100 1863 1/4 Quarter Forward 5 1.625 8.466% 0.015% 101 1869 1/2 Half Reverse 6 1.750 9.118% 0.007% 110 1882

1 Full Forward 7 2.000 10.420% 0.010% 111 1908

Desired Angle

Angular Direction Setting #

Pulse Width Duty Cycle

Duty Cycle Error

3-bit Value

# of Cycles

N/A No Signal 0 0 0% 0% 000 N/A -45 Left 45° 3 1.250 6.513% -0.002% 011 1830 -20 Left 20° 2 1.389 7.236% -0.009% 010 1845 -10 Left 10° 1 1.444 7.526% -0.030% 001 1851 0 Home Position 4 1.500 7.815% -0.002% 100 1856 10 Right 10° 5 1.556 8.104% 0.014% 101 1862 20 Right 20° 6 1.611 8.394% 0.006% 110 1868 45 Right 45° 7 1.750 9.118% 0.001% 111 1882

C - 56

Figure C-1. Overall VHDL schematic including servo controllers and clock.

C - 57

Figure C-2. Oscilloscope screen capture of a 1.5 ms pulse train every 20 ms.

Figure C-3. Oscilloscope screen capture of a 1.75 ms pulse train every 20 ms.

C - 58

Figure C-4. Oscilloscope screen capture of a 2 ms pulse train every 20 ms.

1 library IEEE;2 use IEEE.STD_LOGIC_1164.all;3 use IEEE.STD_LOGIC_ARITH.all;4 use IEEE.STD_LOGIC_UNSIGNED.all;56 ENTITY clk_div IS78 PORT9 (

10 clock_33Mhz : IN STD_LOGIC;11 clock_1MHz : OUT STD_LOGIC;12 clock_100KHz : OUT STD_LOGIC;13 clock_10KHz : OUT STD_LOGIC;14 clock_1KHz : OUT STD_LOGIC;15 clock_100Hz : OUT STD_LOGIC;16 clock_10Hz : OUT STD_LOGIC;17 clock_1Hz : OUT STD_LOGIC);1819 END clk_div;2021 ARCHITECTURE a OF clk_div IS2223 SIGNAL count_1Mhz: STD_LOGIC_VECTOR(4 DOWNTO 0);24 SIGNAL count_100Khz, count_10Khz, count_1Khz : STD_LOGIC_VECTOR(2 DOWNTO 0);25 SIGNAL count_100hz, count_10hz, count_1hz : STD_LOGIC_VECTOR(2 DOWNTO 0);26 SIGNAL clock_1Mhz_int, clock_100Khz_int, clock_10Khz_int, clock_1Khz_int:

STD_LOGIC;27 SIGNAL clock_100hz_int, clock_10Hz_int, clock_1Hz_int : STD_LOGIC;28 BEGIN29 PROCESS30 BEGIN31 -- Divide by 2532 WAIT UNTIL clock_33Mhz'EVENT and clock_33Mhz = '1';33 IF count_1Mhz < 32 THEN34 count_1Mhz <= count_1Mhz + 1;35 ELSE36 count_1Mhz <= "00000";37 END IF;38 IF count_1Mhz < 16 THEN39 clock_1Mhz_int <= '0';40 ELSE41 clock_1Mhz_int <= '1';42 END IF;4344 -- Ripple clocks are used in this code to save prescalar hardware45 -- Sync all clock prescalar outputs back to master clock signal46 clock_1Mhz <= clock_1Mhz_int;47 clock_100Khz <= clock_100Khz_int;48 clock_10Khz <= clock_10Khz_int;49 clock_1Khz <= clock_1Khz_int;50 clock_100hz <= clock_100hz_int;51 clock_10hz <= clock_10hz_int;52 clock_1hz <= clock_1hz_int;53 END PROCESS;5455 -- Divide by 1056 PROCESS57 BEGIN58 WAIT UNTIL clock_1Mhz_int'EVENT and clock_1Mhz_int = '1';59 IF count_100Khz /= 4 THEN60 count_100Khz <= count_100Khz + 1;61 ELSE62 count_100khz <= "000";63 clock_100Khz_int <= NOT clock_100Khz_int;64 END IF;65 END PROCESS;66

67 -- Divide by 1068 PROCESS69 BEGIN70 WAIT UNTIL clock_100Khz_int'EVENT and clock_100Khz_int = '1';71 IF count_10Khz /= 4 THEN72 count_10Khz <= count_10Khz + 1;73 ELSE74 count_10khz <= "000";75 clock_10Khz_int <= NOT clock_10Khz_int;76 END IF;77 END PROCESS;7879 -- Divide by 1080 PROCESS81 BEGIN82 WAIT UNTIL clock_10Khz_int'EVENT and clock_10Khz_int = '1';83 IF count_1Khz /= 4 THEN84 count_1Khz <= count_1Khz + 1;85 ELSE86 count_1khz <= "000";87 clock_1Khz_int <= NOT clock_1Khz_int;88 END IF;89 END PROCESS;9091 -- Divide by 1092 PROCESS93 BEGIN94 WAIT UNTIL clock_1Khz_int'EVENT and clock_1Khz_int = '1';95 IF count_100hz /= 4 THEN96 count_100hz <= count_100hz + 1;97 ELSE98 count_100hz <= "000";99 clock_100hz_int <= NOT clock_100hz_int;

100 END IF;101 END PROCESS;102103 -- Divide by 10104 PROCESS105 BEGIN106 WAIT UNTIL clock_100hz_int'EVENT and clock_100hz_int = '1';107 IF count_10hz /= 4 THEN108 count_10hz <= count_10hz + 1;109 ELSE110 count_10hz <= "000";111 clock_10hz_int <= NOT clock_10hz_int;112 END IF;113 END PROCESS;114115 -- Divide by 10116 PROCESS117 BEGIN118 WAIT UNTIL clock_10hz_int'EVENT and clock_10hz_int = '1';119 IF count_1hz /= 4 THEN120 count_1hz <= count_1hz + 1;121 ELSE122 count_1hz <= "000";123 clock_1hz_int <= NOT clock_1hz_int;124 END IF;125 END PROCESS;126127 END a;128129

1 ----------------------------------------2 ------ PWM for Speed Control ------3 ----------------------------------------45 LIBRARY IEEE;6 USE IEEE.STD_LOGIC_1164.ALL;7 USE IEEE.STD_LOGIC_ARITH.ALL;8 USE IEEE.STD_LOGIC_UNSIGNED.ALL;9

10 ENTITY speed_control IS11 PORT (clock_100kHz : IN STD_LOGIC;12 speed : IN STD_LOGIC_VECTOR( 2 DOWNTO 0 );13 speed_pwm : OUT STD_LOGIC);1415 END speed_control;1617 ARCHITECTURE servo_controller OF speed_control IS18 SIGNAL count_motor: STD_LOGIC_VECTOR( 10 DOWNTO 0 );19 BEGIN20 PROCESS21 BEGIN2223 -- Count_motor is a 20ms timer => [ 0 , 2000 ] based on a true 100kHz clock --24 WAIT UNTIL clock_100kHz'EVENT AND clock_100kHz = '1';25 IF count_motor /= 2000 THEN26 count_motor <= count_motor + 1;27 ELSE28 count_motor <= "00000000000";29 END IF;3031 -- Speed Setting #1 of 8 => BRAKE or OFF = 000 --32 IF count_motor >= 1700 AND count_motor < 1838 THEN33 IF speed = "000" THEN34 speed_pwm <= '0';35 ELSE36 speed_pwm <= '1';37 END IF;3839 -- At this point the pulse is already 1 ms long --40 -- Decide how long to allow pulse to remain high. [ 1ms , 2ms ] --4142 -- Speed Setting #2 of 8 => Quarter Reverse (CW) --43 ELSIF count_motor >= 1838 AND count_motor < 1844 THEN44 IF speed /="000" THEN45 CASE speed IS46 WHEN "010" =>47 speed_pwm <= '0';48 WHEN "001" =>49 speed_pwm <= '1';50 WHEN "011" =>51 speed_pwm <= '1';52 WHEN "100" =>53 speed_pwm <= '1';54 WHEN "101" =>55 speed_pwm <= '1';56 WHEN "110" =>57 speed_pwm <= '1';58 WHEN "111" =>59 speed_pwm <= '1';60 WHEN OTHERS => NULL;61 END CASE;62 ELSE63 speed_pwm <= '0';64 END IF;6566 -- Speed Setting #3 of 8 => Eighth Reverse (CW) --67 ELSIF count_motor >= 1844 AND count_motor < 1850 THEN

68 IF speed /="000" THEN69 CASE speed IS70 WHEN "010" =>71 speed_pwm <= '0';72 WHEN "001" =>73 speed_pwm <= '0';74 WHEN "011" =>75 speed_pwm <= '1';76 WHEN "100" =>77 speed_pwm <= '1';78 WHEN "101" =>79 speed_pwm <= '1';80 WHEN "110' =>81 speed_pwm <= '1';82 WHEN "111" =>83 speed_pwm <= '1';84 WHEN OTHERS => NULL;85 END CASE;86 ELSE87 speed_pwm <= '0';88 END IF;8990 -- Speed Setting #4 of 8 => Neutral (1.5 ms pulse) --91 ELSIF count_motor >= 1850 AND count_motor < 1856 THEN92 IF speed /="000" THEN93 CASE speed IS94 WHEN "010" =>95 speed_pwm <= '0';96 WHEN "001" =>97 speed_pwm <= '0';98 WHEN "011" =>99 speed_pwm <= '0';

100 WHEN "100" =>101 speed_pwm <= '1';102 WHEN "101" =>103 speed_pwm <= '1';104 WHEN "110" =>105 speed_pwm <= '1';106 WHEN "111" =>107 speed_pwm <= '1';108 WHEN OTHERS => NULL;109 END CASE;110 ELSE111 speed_pwm <= '0';112 END IF;113114 -- Speed Setting #5 of 8 => Eighth Forward (CCW) --115 ELSIF count_motor >= 1856 AND count_motor < 1863 THEN116 IF speed /="000" THEN117 CASE speed IS118 WHEN "010" =>119 speed_pwm <= '0';120 WHEN "001" =>121 speed_pwm <= '0';122 WHEN "011" =>123 speed_pwm <= '0';124 WHEN "100" =>125 speed_pwm <= '0';126 WHEN "101" =>127 speed_pwm <= '1';128 WHEN "110" =>129 speed_pwm <= '1';130 WHEN "111" =>131 speed_pwm <= '1';132 WHEN OTHERS => NULL;133 END CASE;134 ELSE

135 speed_pwm <= '0';136 END IF;137138 -- Speed Setting #6 of 8 => Quarter Forward (CCW) --139 ELSIF count_motor >= 1863 AND count_motor < 1875 THEN140 IF speed /="000" THEN141 CASE speed IS142 WHEN "010" =>143 speed_pwm <= '0';144 WHEN "001" =>145 speed_pwm <= '0';146 WHEN "011" =>147 speed_pwm <= '0';148 WHEN "100" =>149 speed_pwm <= '0';150 WHEN "101" =>151 speed_pwm <= '0';152 WHEN "110" =>153 speed_pwm <= '1';154 WHEN "111" =>155 speed_pwm <= '1';156 WHEN OTHERS => NULL;157 END CASE;158 ELSE159 speed_pwm <= '0';160 END IF;161162 -- Speed Setting #7 of 8 => Half Forward (CCW) --163 ELSIF count_motor >= 1875 AND count_motor < 1900 THEN164 IF speed /="000" THEN165 CASE speed IS166 WHEN "010" =>167 speed_pwm <= '0';168 WHEN "001" =>169 speed_pwm <= '0';170 WHEN "011" =>171 speed_pwm <= '0';172 WHEN "100" =>173 speed_pwm <= '0';174 WHEN "101" =>175 speed_pwm <= '0';176 WHEN "110" =>177 speed_pwm <= '0';178 WHEN "111" =>179 speed_pwm <= '1';180 WHEN OTHERS => NULL;181 END CASE;182183 -- Speed Setting #8 of 8 => Full Forward (CCW) --184 ELSE185 speed_pwm <= '0';186 END IF;187188 ELSE189 speed_pwm <= '0';190 END IF;191192 END PROCESS;193 END servo_controller;194195 -----------------------------------------------------------------------------196 --All of the case statements with <=1 and reoccurring <=0 could be deleted.--197 -----------------------------------------------------------------------------

1 --------------------------------------------------2 ------ PWM for Servo Direction Control ------3 --------------------------------------------------45 LIBRARY IEEE;6 USE IEEE.STD_LOGIC_1164.ALL;7 USE IEEE.STD_LOGIC_ARITH.ALL;8 USE IEEE.STD_LOGIC_UNSIGNED.ALL;9

10 ENTITY direction_control IS11 PORT (clock_100kHz : IN STD_LOGIC;12 position : IN STD_LOGIC_VECTOR( 2 DOWNTO 0 );13 direction_pwm : OUT STD_LOGIC);1415 END direction_control;1617 ARCHITECTURE servo_controller OF direction_control IS18 SIGNAL count_motor: STD_LOGIC_VECTOR( 10 DOWNTO 0 );19 BEGIN20 PROCESS21 BEGIN22 -- Count_motor is a 20ms timer => [ 0 , 2000 ] --23 WAIT UNTIL clock_100kHz'EVENT AND clock_100kHz = '1';24 IF count_motor /= 2000 THEN25 count_motor <= count_motor + 1;26 ELSE27 count_motor <= "00000000000";28 END IF;2930 -- Direction Setting #1 of 8 => SIGNAL OFF = 000 --31 IF count_motor >= 1700 AND count_motor < 1818 THEN32 IF position = "000" THEN33 direction_pwm <= '0';34 ELSE35 direction_pwm <= '1';36 END IF;3738 -- At this point the pulse is already 1 ms long --39 -- Decide how long to allow pulse to remain high. [ 1ms , 2ms ] --4041 -- Direction Setting #2 of 8 => 45 degrees LEFT or CW from center (1830) --42 -- After trial and error the usable value was determined to be 1818 --43 ELSIF count_motor >= 1818 AND count_motor < 1830 THEN44 IF position /="000" THEN45 CASE position IS46 WHEN "011" =>47 direction_pwm <= '0';48 WHEN "010" =>49 direction_pwm <= '1';50 WHEN "001" =>51 direction_pwm <= '1';52 WHEN "100" =>53 direction_pwm <= '1';54 WHEN "101" =>55 direction_pwm <= '1';56 WHEN "110" =>57 direction_pwm <= '1';58 WHEN "111" =>59 direction_pwm <= '1';60 WHEN OTHERS => NULL;61 END CASE;62 ELSE63 direction_pwm <= '0';64 END IF;6566 -- Direction Setting #3 of 8 => 20 degrees LEFT or CW from center (1845) --

67 -- After trial and error the usable value was determined to be 1830 --68 ELSIF count_motor >= 1830 AND count_motor < 1838 THEN69 IF position /="000" THEN70 CASE position IS71 WHEN "011" =>72 direction_pwm <= '0';73 WHEN "010" =>74 direction_pwm <= '0';75 WHEN "001" =>76 direction_pwm <= '1';77 WHEN "100" =>78 direction_pwm <= '1';79 WHEN "101" =>80 direction_pwm <= '1';81 WHEN "110" =>82 direction_pwm <= '1';83 WHEN "111" =>84 direction_pwm <= '1';85 WHEN OTHERS => NULL;86 END CASE;87 ELSE88 direction_pwm <= '0';89 END IF;9091 -- Direction Setting #4 of 8 => 10 degrees LEFT or CW from center (1850) --92 -- After trial and error the usable value was determined to be 1838 --93 ELSIF count_motor >= 1838 AND count_motor < 1845 THEN94 IF position /="000" THEN95 CASE position IS96 WHEN "011" =>97 direction_pwm <= '0';98 WHEN "010" =>99 direction_pwm <= '0';

100 WHEN "001" =>101 direction_pwm <= '0';102 WHEN "100" =>103 direction_pwm <= '1';104 WHEN "101" =>105 direction_pwm <= '1';106 WHEN "110" =>107 direction_pwm <= '1';108 WHEN "111" =>109 direction_pwm <= '1';110 WHEN OTHERS => NULL;111 END CASE;112 ELSE113 direction_pwm <= '0';114 END IF;115116 -- Direction Setting #5 of 8 => 0 degrees or center (1856) --117 -- After trial and error the usable value was determined to be 1845 --118 ELSIF count_motor >= 1845 AND count_motor < 1862 THEN119 IF position /="000" THEN120 CASE position IS121 WHEN "011" =>122 direction_pwm <= '0';123 WHEN "010" =>124 direction_pwm <= '0';125 WHEN "001" =>126 direction_pwm <= '0';127 WHEN "100" =>128 direction_pwm <= '0';129 WHEN "101" =>130 direction_pwm <= '1';131 WHEN "110" =>132 direction_pwm <= '1';

133 WHEN "111" =>134 direction_pwm <= '1';135 WHEN OTHERS => NULL;136 END CASE;137 ELSE138 direction_pwm <= '0';139 END IF;140141 -- Direction Setting #6 of 8 => 10 degrees RIGHT or CCW from center (1862) --142 ELSIF count_motor >= 1862 AND count_motor < 1868 THEN143 IF position /="000" THEN144 CASE position IS145 WHEN "011" =>146 direction_pwm <= '0';147 WHEN "010" =>148 direction_pwm <= '0';149 WHEN "001" =>150 direction_pwm <= '0';151 WHEN "100" =>152 direction_pwm <= '0';153 WHEN "101" =>154 direction_pwm <= '0';155 WHEN "110" =>156 direction_pwm <= '1';157 WHEN "111" =>158 direction_pwm <= '1';159 WHEN OTHERS => NULL;160 END CASE;161 ELSE162 direction_pwm <= '0';163 END IF;164165 -- Direction Setting #7 of 8 => 20 degrees CCW from center (1868) --166 ELSIF count_motor >= 1868 AND count_motor < 1882 THEN167 IF position /="000" THEN168 CASE position IS169 WHEN "011" =>170 direction_pwm <= '0';171 WHEN "010" =>172 direction_pwm <= '0';173 WHEN "001" =>174 direction_pwm <= '0';175 WHEN "100" =>176 direction_pwm <= '0';177 WHEN "101" =>178 direction_pwm <= '0';179 WHEN "110" =>180 direction_pwm <= '0';181 WHEN "111" =>182 direction_pwm <= '1';183 WHEN OTHERS => NULL;184 END CASE;185 ELSE186 direction_pwm <= '0';187 END IF;188189 -- Direction Setting #8 of 8 => 45 degrees CCW from center (1882) --190 ELSE191 direction_pwm <= '0';192 END IF;193194 END PROCESS;195 END servo_controller;196 -----------------------------------------------------------------------------197 --All of the case statements with <=1 and reoccurring <=0 could be deleted.--198 -----------------------------------------------------------------------------

1 #include "nios.h"2 #include "vicon.h"34 /*-----------------------------------------------------------------5 * Author(s): Kwabena Asare Bosompem6 * Andre Moore7 * Jeff Vickers8 * Kevin Walker9 * Created: May 30th, 2002.

10 * Modified: July 15th, 2002.11 * Description: Functions for Servo Control12 * Comments:13 *-----------------------------------------------------------------*/14151617 /*-----------------------------------------------------------------*18 * File Wide Variables *19 *-----------------------------------------------------------------*/20 int viconspeed;21 int vicondirection;22 extern np_pio *direction;23 extern np_pio *speed ;242526 /*-----------------------------------------------------------------*27 * Accessors *28 *-----------------------------------------------------------------*/29 /**30 * Gets the servo's current speed and returns it as an int31 * Created: June 6th, 200232 * Last Modified: June 6th, 200233 * Comments:Implementation to be discussed34 *35 */36 int GetSpeed(void)37 {38 return viconspeed;3940 }//end of GetSpeed414243 /**44 * Gets the servo's current direction and returns it as a short45 * or preferably a binary number46 * Created: June 6th, 200247 * Last Modified: June 6th, 200248 * Comments:Implementation to be discussed49 *50 */51 int GetDirection(void)52 {53 return vicondirection;5455 }//end of GetDirection565758 /*-----------------------------------------------------------------*59 * Modifiers *60 *-----------------------------------------------------------------*/61 /**62 * Sets the servo's speed based on integer passed in63 * Created: June 6th, 200264 * Last Modified: July 15th, 200265 * Comments: Acceptable range 0-766 */67 void SetSpeed(int spd)

68 {69 if(spd >7)70 spd = 7;71 else if(spd < 0)72 spd = 0;7374 viconspeed = spd;75 speed = (np_pio *)0x430;76 speed->np_piodata = viconspeed;7778 }//end of SetSpeed79808182 /**83 * Sets the servo's direction based on the integer passed in84 * Created: June 6th, 200285 * Last Modified: July 15th, 200286 * Comments: Acceptable range 0-787 *88 */89 void SetDirection(int drx)90 {91 if(drx >7)92 drx = 7;93 else if(drx < 0)94 drx = 0;9596 vicondirection = drx ;97 direction = (np_pio *)0x490;98 direction->np_piodata = vicondirection;99

100 }//end of SetDirection101

D - 69

Appendix D:

State Machine C Code & Track

D - 70

Figure D-1. ViCoN-Bot negotiating a left hand turn.

Figure D-2. Part of ViCoN testing course.

1 #include "nios.h"2 #include "vicon.h"3 #include "pio_lcd16207.h"4 /*-----------------------------------------------------------------5 *6 * Author(s): Kwabena Asare Bosompem7 * Andre Moore8 * Jeff Vickers9 * Kevin Walker

10 * Created: June 10th, 200211 * Modified: July 18th, 200212 * Description: Controls the different states needed to use the13 * camera as tracking utility for the robot14 *-----------------------------------------------------------------*/151617 char state;18 int mass = 8; // Value to be used to set the Middle Mass19 int count = 0;20 int idle_count = 0; // Global used in camera idle21 Color color_k;22 Color color_checkup;23 int packets = 2; // Number of packets used in Tracking()24 np_pio *direction = na_direction_pio;25 np_pio *speed = na_speed_pio;26 int TALK = 0; // 1 is for DEBUG mode2728 int direction0 = 0;//Stop or Brake?29 int direction1 = 1;//-4530 int direction2 = 2;//-2031 int direction3 = 3;//-1032 int direction4 = 4;//033 int direction5 = 5;//1034 int direction6 = 6;//2035 int direction7 = 7;//453637 int speed0 = 0; //Reverse 1/838 int speed1 = 1; //Reverse 1/439 int speed2 = 2; //Stop40 int speed3 = 3; //Neutral41 int speed4 = 4; //Foreward 1/842 int speed5 = 5; //Foreward 1/443 int speed6 = 6; //Foreward 1/244 int speed7 = 7; //Foreward 14546 extern int pollmode;47 extern SdataPacket spacket;48 extern CdataPacket cpacket;49 extern MdataPacket mpacket;50 extern NdataPacket npacket;51525354 //Color PINK = {220, 240, 10, 60, 10, 40};55 //Color YELLOW = {235, 240, 235, 240, 40, 80};56 //Color GREEN = {75, 120, 140, 240, 40, 100};575859 typedef struct {60 int TurnDirection; // number that will hold the direction of the servo61 int DriveSpeed; // number that will hold the speed of the servo62 }ServoValue;63 ServoValue myServoValue;646566 /**67 * M A I N ( V O I D )

68 * Description: Main function for the state machine. Initializes the LCD andstarts the

69 * first state70 * Created: June 15th, 2002.71 * Paramaters:72 * Returns:73 * Comments:74 * Modified:75 */76 int main(void){7778 np_pio *pio = na_button_pio; //choose the sw buttons as input7980 //set up interrupt handler. this responds to the pushbuttons81 nr_installuserisr(na_button_pio_irq,MyPIO_ISR,(int)pio);8283 nr_pio_lcdinit(na_lcd_pio); // Initialize LCD848586 pio->np_pioedgecapture = 0; // clear any existing IRQ87 pio->np_piodirection = 0; // all input88 pio->np_piointerruptmask = 0xff; // they all generate irq's8990 nr_pio_lcdwritescreen("GaTech ViCoN-Bot"); //Welcome Message91 nr_delay(2000);92 CameraStart();9394 }959697 /**98 * C A M E R A S T A R T ( V O I D )99 * Description: Set's the cameras initialization (MM and Poll)

100 * Created: June 15th, 2002.101 * Paramaters:102 * Returns:103 * Comments:104 * Modified:105 */106 int CameraStart(void){107108 int ans1;109 int ans2;110 int ans3;111 char tempc[32];112113 nr_pio_lcdwritescreen("Camera Initialized");114 nr_delay(1000);115116 do{117 ResetCamera();118 ans2 = SetPollMode(1);119 ans1 = SetMiddleMass(mass);120 ans3 = SetColorMode(44);121 }while (ans1 == FAILURE);122123 sprintf(tempc, "%d %d %d", ans1, ans2, ans3);124 nr_pio_lcdwritescreen(tempc);125 nr_delay(1000);126 SetSpeed(NEUTRAL); // Set the motor to neutral127 SetDirection(STRAIGHT); // Set the wheels straight128 color_checkup = PINK;129 FindColor();130 }131132133 /**

134 * F I N D C O L O R ( V O I D )135 * Description: Set's the camera in a loop to find an initial color to track136 * Created: June 15th, 2002.137 * Paramaters:138 * Returns:139 * Comments:140 * Modified:141 */142 int FindColor(void){143144 char nicon[32];145 char kevin[32];146 int count2 = 0;147148 int conf[3] = {0, 0, 0};149 int conf_value;150151 if (count != 2)152 {153 count++;154 }155 else156 {157 count = 0;158 }159 SetSpeed(NEUTRAL);160 SetDirection(STRAIGHT);161162 // Add an additional case for another color163 switch (count)164 {165 case(1):166 //color_k = PINK;167 color_k = GREEN;168169 if (TALK == 1) {170 nr_pio_lcdwritescreen("Color is now GREEN");171 nr_delay(1100);}172173 break;174 case(2):175 //color_k = YELLOW;176 color_k = PINK;177178 if (TALK == 1) {179 nr_pio_lcdwritescreen("Color is now PINK");180 nr_delay(1100);}181182 break;183 case(0):184 color_k = PINK;185186187 if (TALK == 1){188 nr_pio_lcdwritescreen("Color is now YELLOW");189 nr_delay(1100);}190191 break;192 default:193 break;194 }195196 // This do loop controls how many packets are need to find if a color is present197 // Testing has shown that only one packet is needed and exhibits the best

functionality198 //do{199

200201 TrackColor(color_k);202203 // conf[count2] = npacket.confidence; Use if more than one packet is used204205206 if (TALK == 1) {207 sprintf(kevin, "Confidence: %d Pixels:%d", npacket.confidence, npacket.

pixels);208 nr_pio_lcdwritescreen(kevin);209 nr_delay(1000);}210211 // Use if tracking more than one packet212 // if (count2 == 2)213 // {214 // conf_value = ((conf[0] + conf[1] + conf[2]) / 3);215 //216 // if (TALK == 1) {217 // sprintf(kevin, "%d %d %d", conf[0], conf[1], conf[2]);218 // nr_pio_lcdwritescreen(kevin);219 // nr_delay(800);}220 //221 // count2 = count2 + 1;222 // }223 // else224 // {225 // count2 = count2 + 1;226 //227 // }228 //229 // }while(count2 < 3);230231232 count2=0;233 conf_value=npacket.confidence;234235 if (TALK == 1) {236 sprintf(kevin, "average confidence: %d", conf_value);237 nr_pio_lcdwritescreen(kevin);238 nr_delay(1000);}239240 if (conf_value > 20)241 {242 if (TALK == 1) {243 nr_pio_lcdwritescreen("Tracking State");244 nr_delay(500);}245 TrackState();246247 }248 else249 {250 if (TALK == 1) {251 nr_pio_lcdwritescreen("Finding Color");252 nr_delay(500);}253 FindColor();254255 }256 }257258 /**259 * T R A C K I N G S T A T E ( I N T )260 * Description: Tracks a color261 * Created: June 15th, 2002.262 * Paramaters:263 * Returns:264 * Comments:265 * Modified:

266 */267 int TrackState(void){268269 char kevin[32];270271 int countk = 0;272 int checkup = 0;273274 int state_conf = 0;275 int state_mx = 0;276 int state_my = 0;277 int state_x1 = 0;278 int state_y1 = 0;279 int state_x2 = 0;280 int state_y2 = 0;281 int state_pixel = 0;282 int state_confidence = 0;283284285 if (TALK == 1) {286 nr_pio_lcdwritescreen("Tracking");287 nr_delay(500);288 }289290 if (CompareColors(color_k, color_checkup) == SUCCESS){291 TrackColor(color_checkup);292 checkup = npacket.confidence;293 SetDirection(STRAIGHT);294 //SetSpeed(EIGHTH_FORWARD);295 if (checkup <= 20) {FindColor();};296 }297298 do{299 TrackColor(color_k);300 state_conf += npacket.confidence;301 state_mx += npacket.mx;302 state_my += npacket.my;303 state_x1 += npacket.x1;304 state_x2 += npacket.x2;305 state_y1 += npacket.y1;306 state_y2 += npacket.y2;307 state_pixel += npacket.pixels;308 countk++;309310 }while(countk < packets);311 state_conf = (state_conf/packets);312 state_mx = (state_mx/packets);313 state_my = (state_my/packets);314 state_pixel = (state_pixel/packets);315316 if (TALK == 1) {317 sprintf(kevin, "T:..: con:%d pix:%d", state_conf, state_pixel);318 nr_pio_lcdwritescreen(kevin);319 nr_delay(800);}320321 if (state_conf > 20) {DataControl(state_conf, state_mx, state_my, state_pixel

);} else {FindColor();}322 }323324325326 /**327 * C O M P A R E C O L O R S ( S T R U C T , S T R U C T )328 * Description: Compares two colors329 * Created: June 15th, 2002.330 * Paramaters: Two structures (Color)331 * Returns: int (SUCCESS or FAILURE)

332 * Comments:333 * Modified:334 */335 int CompareColors(Color c1, Color c2)336 {337338 if(c1.rmin != c2.rmin)339 return FAILURE;340 else if (c1.bmin != c2.bmin)341 return FAILURE;342 else if (c1.gmin != c2.gmin)343 return FAILURE;344 else if (c1.rmax != c2.rmax)345 return FAILURE;346 else if (c1.gmax != c2.gmax)347 return FAILURE;348 else if (c1.bmax != c2.bmax)349 return FAILURE;350 else351 return SUCCESS;352353 }//end of CompareColors354355356357 /**358 * D A T A C O N T R O L ( I N T , I N T , I N T , I N T )359 * Description: DataControl will serve as a function to calculate the distance,360 * direction, and speed that the ViConBot will need to control the

servos.361 * Created: June 15th, 2002.362 * Paramaters:363 * Returns:364 * Comments:365 * Modified:366 */367 int DataControl(int con, int m1, int m2, int pix){368369 char kevinw[32];370371 if (CompareColors(color_k, PINK)== SUCCESS){372373 myServoValue.TurnDirection = SetViconDirection(m1);374375 if (pix >= 180)376 myServoValue.DriveSpeed = EIGHTH_FORWARD;377 else if ((pix < 180) && (pix >= 25))378 myServoValue.DriveSpeed = EIGHTH_FORWARD;379 else380 myServoValue.DriveSpeed = EIGHTH_FORWARD;381382383384 }else if (CompareColors(color_k, GREEN)== SUCCESS){385386 if (pix >= 200){387 myServoValue.DriveSpeed = EIGHTH_REVERSE;388 myServoValue.TurnDirection = SetViconDirection();}389 else if ((pix < 200) && (pix >= 125)){390 myServoValue.DriveSpeed = EIGHTH_FORWARD;391 myServoValue.TurnDirection = LEFT_45;}392 else if ((pix < 125) && (pix >= 25)){393 myServoValue.DriveSpeed = EIGHTH_FORWARD;394 myServoValue.TurnDirection = LEFT_20;}395 else {396 myServoValue.DriveSpeed = EIGHTH_FORWARD;397 myServoValue.TurnDirection = SetViconDirection();}

398399 }else{400401 myServoValue.TurnDirection = SetViconDirection(m1);402403 if (pix >= 80)404 myServoValue.DriveSpeed = NEUTRAL;405 else if ((pix < 80) && (pix >= 40))406 myServoValue.DriveSpeed = EIGHTH_FORWARD;407 else408 myServoValue.DriveSpeed = EIGHTH_FORWARD;409 }410411412 if (TALK == 1) {413 sprintf(kevinw, "Direction:%d .::. Speed:%d", myServoValue.TurnDirection,

myServoValue.DriveSpeed);414 nr_pio_lcdwritescreen(kevinw);415 nr_delay(500);416 }417418 SetDirection(myServoValue.TurnDirection);419 SetSpeed(myServoValue.DriveSpeed);420 TrackState();421 }422423424425 int SetViconDirection(int direc_value){426427428 if (direc_value >= 75)429430 return RIGHT_20;431432 else if ((direc_value < 75) && (direc_value >= 65))433434 return RIGHT_20;435436 else if ((direc_value < 70) && (direc_value >= 60))437438 return RIGHT_10;439440 else if ((direc_value < 60) && (direc_value >= 40))441442 return STRAIGHT;443444 else if ((direc_value < 40) && (direc_value >= 30))445446 return LEFT_10;447448 else if ((direc_value < 30) && (direc_value >= 20))449450 return LEFT_20;451452 else453454 return LEFT_45;455456 }//End of SetViconDirection457458459460 /**461 * R E S E T ( V O I D )462 * Description: Reset will reset the camera and send it back to the initial state

of CameraStart

463 * Created: June 15th, 2002.464 * Paramaters:465 * Returns:466 * Comments:467 * Modified:468 */469 int Reset(void){470 int startover;471 do{472 startover = ResetCamera();473 }while(startover == FAILURE);474 CameraStart();475 }476477 /**478 * I D L E R O B O T ( I N T )479 * Description: Sets the robot in an idle state until it receives a second second

input from the nios480 * pushbutton481 * Created: July 17, 2002482 * Paramaters:483 * Returns:484 * Comments:485 * Modified:486 */487 int IdleRobot(int value){488489 idle_count += value;490491 if (idle_count == 2)492 {493 idle_count = 0;494 FindColor();495 }else496 {497 SetDirection(STRAIGHT);498 SetSpeed(NEUTRAL);499 };500501 }502503504505 void MyPIO_ISR(int context)506 {507 np_pio *pio = (np_pio *)context;508 char s[32];509 int buttons;510 pio->np_pioedgecapture = 0; //reset interrupt511 buttons = pio->np_piodata; //get button pressed512 sprintf(s,"Button Pressed: %1x",buttons & 0xf);513 if(s[19]!='f')514 {515 nr_pio_lcdwritescreen(s);516 nr_delay(500);517518 if (s[19]=='b'){519 nr_pio_lcdwritescreen("Return to CameraStart");520 nr_delay(1500);521 CameraStart();522 }523 else if(s[19]=='7'){524 nr_pio_lcdwritescreen("Start Travelen Man!");525 nr_delay(1500);526 TravelFind();527 }528 else if(s[19]=='d'){

529 nr_pio_lcdwritescreen("Set Robot Idle");530 nr_delay(1500);531 IdleRobot(1);532 }533 else if(s[19]=='e'){534 nr_pio_lcdwritescreen("Reset");535 nr_delay(1500);536 //if(currentmode == 1)537 // currentmode = 0;538 //else539 // currentmode = 1;540 //SetPollMode(currentmode);541 Reset();542 }543544 }545546 }547548

E - 80

Appendix E:

ViCoN-Bot Data & Pictures

E - 81

Table E-1. ViCoN-Bot Part List.

Quantity Part

1 Tamiya 1/12 ScaleM1025 Hummer 1 Futaba MC330CR FET Speed Control 1 Altera Nios Board ( including equipment & software ) 1 CMUcam (including software) 1 Null Modem ( 2 male serial connectors ) 2 7.2 volt 2000mAh batteries 4 1.5 volt batteries & assembly 1 5 volt ½ amp regulator 1 100 µF Electrolytic Capacitor 1 100 pF Polystyrene Capacitor

Neon Pink & Green Poster Boards Mounting Hardware ( i.e. L-bracket, screws, tie wraps )

Figure E-1. Nios board and LCD mounted on M1025.

E - 82

Figure E-2. CMUcam mounted on robot.

Figure E-3. Under the hood and chassis view of robot.

E - 83

Figure E-4. Frontal picture of robot.

Figure E-5. Top view of robot.