purple haze - kth€¦ · this report describes the design of a small autonomous hockey-playing...

Purple HazeProject Report in 2D1426 Robotics and Autonomous Systems

Niklas Lundström Rebecca Steinert Dirk [email protected] [email protected] [email protected]

12th November 2003

This report describes the design of a small autonomous hockey-playing robot,Purple Haze. It starts with a short description of the hockey rules and their influenceon the design in the form of basic design considerations. The final mechanical andelectrical construction is explained in detail, followed by a concise description of thesoftware architecture and implementation. The report closes with an analysis of therobot performance during the final robot competition.

The project was part of the course 2D1426 Robotics and Autonomous Systems

held at Kungliga Tekniska Högskolan (KTH) in Stockholm, Sweden, in autumn2003.

Contents

1. Introduction 71.1. Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71.2. Robot Hockey . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

1.2.1. Robot Constraints . . . . . . . . . . . . . . . . . . . . . . 71.2.2. The Hockey Rink and Puck . . . . . . . . . . . . . . . . . 81.2.3. The Game . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2. Mechanical design 102.1. Design targets . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.2. Construction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.2.1. Lower plane . . . . . . . . . . . . . . . . . . . . . . . . . 112.2.2. Upper plane . . . . . . . . . . . . . . . . . . . . . . . . . 122.2.3. Casing . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

3. Sensing 153.1. Reflex Detectors and Floor Sensing . . . . . . . . . . . . . . . . . 153.2. IR-Sensors - navigation and puck sensing . . . . . . . . . . . . . . 173.3. Motor Sensing . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173.4. Threshold experiments . . . . . . . . . . . . . . . . . . . . . . . . 18

4. Locomotion and manipulation 20

5. Software and control 225.1. Development environment . . . . . . . . . . . . . . . . . . . . . . 225.2. Design overview . . . . . . . . . . . . . . . . . . . . . . . . . . . 225.3. Subsumption architecture . . . . . . . . . . . . . . . . . . . . . . 22

3

Contents

5.3.1. Behaviour . . . . . . . . . . . . . . . . . . . . . . . . . . 235.3.2. Behaviour cycle . . . . . . . . . . . . . . . . . . . . . . . 23

5.4. Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . 245.4.1. The behaviours . . . . . . . . . . . . . . . . . . . . . . . 24

6. Electronics 286.1. The main board . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

6.1.1. Main processor PIC16F877 . . . . . . . . . . . . . . . . . 286.1.2. Co-processor PIC16F876 . . . . . . . . . . . . . . . . . . 296.1.3. Motor drive circuits . . . . . . . . . . . . . . . . . . . . . 296.1.4. Dip-switches . . . . . . . . . . . . . . . . . . . . . . . . . 29

6.2. Auxiliary boards . . . . . . . . . . . . . . . . . . . . . . . . . . . 296.2.1. RS-232 signal converter . . . . . . . . . . . . . . . . . . . 306.2.2. LED driver board . . . . . . . . . . . . . . . . . . . . . . 306.2.3. Voltage booster board . . . . . . . . . . . . . . . . . . . . 30

6.3. Electrical supply . . . . . . . . . . . . . . . . . . . . . . . . . . . 306.4. Electronic devices and their connections . . . . . . . . . . . . . . 30

6.4.1. Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 306.4.2. Speaker . . . . . . . . . . . . . . . . . . . . . . . . . . . 316.4.3. Reflex detectors . . . . . . . . . . . . . . . . . . . . . . . 316.4.4. Slotted opto detectors . . . . . . . . . . . . . . . . . . . . 316.4.5. IR-sensors . . . . . . . . . . . . . . . . . . . . . . . . . . 316.4.6. LCD display . . . . . . . . . . . . . . . . . . . . . . . . . 31

7. Discussion 327.1. Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

7.1.1. Construction and sensors . . . . . . . . . . . . . . . . . . 327.1.2. Performance . . . . . . . . . . . . . . . . . . . . . . . . . 33

A. Source code 34A.1. settings.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34A.2. main.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36A.3. intr.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56

4

Contents

A.4. pwm.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58A.5. ir_comm.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59A.6. debug.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62A.7. sprint.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63A.8. adc.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64A.9. lcd.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64A.10.defines.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

B. Circuit Diagrams 67

5

List of Figures

1.1. The hockey rink . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.1. Schematic drawing of lower plane and casing . . . . . . . . . . . . 122.2. Puck sensor position and direction . . . . . . . . . . . . . . . . . 13

3.1. Schematic drawing of the sensor positions on Purple Haze . . . . 163.2. An encoder disc used in conjunction with the slotted opto detectors 18

5.1. The state transitions of both avoid goal zone behaviours . . . . . 255.2. The stuck behaviour state transitions . . . . . . . . . . . . . . . . 265.3. The state transitions of find puck behaviour . . . . . . . . . . . . 275.4. The state transitions of to offensive goal behaviour . . . . . . . . 27

B.1. The layout of the robot’s motherboard [3] . . . . . . . . . . . . . 68

6

1. Introduction

1.1. Background

This project is part of the course Robotics and Autonomous Systems, 2D1426, atKTH.

The project task has been to build, in groups of three students, a simple autonom-ous robot that is able to play an easy form of robot hockey. The groups coulddevelop their own design very freely, based on the microcontroller and other mater-ial provided by the course leader. However, the robot hockey rules [3] introducedcertain constraints on the design.

1.2. Robot Hockey

1.2.1. Robot Constraints

In order to be allowed to take part in the robot competition, a robot has to fulfillsome requirements. The vertical projection of the robot’s body (without the stickand very flexible parts) must fit into a circle with a diameter of 25cm. In addition,the body is not allowed to have any concave parts that could be used to guide thepuck.

Guiding the puck is the task of the hockey stick, which also is not allowed tohave any concavities. The stick must be no longer than one puck diameter and atleast 2cm high, but isn’t allowed to screen the IR light emitted from the puck.

Furthermore, it is not allowed to mount any device that could be harmful to theopponent.

7

1. Introduction

1.2.2. The Hockey Rink and Puck

The hockey rink is designed similar to an ice hockey rink. It therefore also hasboards, but differs in the point that the two goals are placed in the board, not infront of it. The rink floor is white, except the black centerline and the two blackgoal zones. The goal itself is marked trough a red line inside the goal zone. Eachgoal features an array of IR LEDs that emit light with a characteristic modulation.Thus, it is possible to distinguish between the two goals. The puck features also aring of IR LEDs with a unique modulation.

Figure 1.1.: The hockey rink

1.2.3. The Game

The game of robot hockey is quite simple. As in most field sports, the object ofthe game is to score more goals than the opponent does during a match. A goalis scored successfully when the puck reaches the red goal line and the attackingrobot’s body doesn’t get into the black goal zone by more than 2,5cm. Moreover,the attacking robot has to signal its scoring attempt by playing a melody. The puckis placed on the centerline after a successful goal. Whilst a robot is - apart froma scoring attempt - allowed to enter the offensive goal zone, it is always punishedby a 10 seconds penalty when it enters its own defensive goal zone without beingpushed in by the opponent.

A team may take their robot out of the rink during a game for reparation. The

8

1. Introduction

robot has to stay out of the rink for at least 30 seconds without regard to the actualrepair duration. However, the game continues during reparation.

9

2. Mechanical design

2.1. Design targets

Our main goal was to build a fast and agile as well as easily maintainable robot.The latter seemed to be crucial because of the obvious need to repair the robotduring the development period as well as the competition, as reported by numerousgroups that took part in the spring course ([4], [5]). Agility and speed were - apartfrom a clever software architecture - chosen to be our approach to a successfulparticipation in the robot competition.

2.2. Construction

On the one hand it seemed crucial to build Purple Haze as stiff as possible in order toachieve a high predictability, accuracy and repeatability of the robots movements.This especially gains in importance when it comes to avoiding driving into theoffensive goal zone but still scoring a valid goal. On the other hand, the robotshould be very lightweight to be able to drive at high speed and have low inertia,as well as constructed in a modular way for easy maintenance.

The demands seem to be somewhat contradicting. Indeed they turned out to beso, when our first approach was to build the robot entirely from Lego because ofthe expected low weight and easy maintainability. Unfortunately, Lego didn’t givethe stiffness and robustness we desired and therefore led to very high friction onthe floor and sluggish movements because of an unpredictable bending of the wheelaxis.

After some experimentation, we ended up with a different construction but keptthe two-plane approach already followed in our Lego construction. The casing ofthe robot was kept separately to get an even easier maintainability and lower the

10


weight because of the possibility to use another material than for the rest of therobot.

2.2.1. Lower plane

Since the lower plane was supposed to carry the whole robot as well as to serveas a mounting for the hockey stick, we decided to use a material that was as hardand stiff as possible. This should also avoid the problems we encountered with ourexperimental Lego construction. The acrylic plate used by this spring’s winningrobot FejkOpaque [4] turned out to meet these needs, but was unfortunately ratherheavy. We therefore decided to cut the lower plane as small as possible and endedup having a rectangular base plate with just enough room for the two motors andthe battery as well as two supporting gliders to prevent the robot from nodding.

We mounted the motors parallel to the floor and on one common axis as nearto each other as possible. Each motor was screwed to the base plate using twoM4 threaded bolts of about 10cm length that also served as mountings for theupper plane. The two motor mountings furthermore carried one relative opticalencoder each, sitting on the wheel side of the transmission. First, we used medium-sized Lego wheels that gave a good relation between possible top speed and force.Unfortunately, these wheels got damaged during our testing and had to be replacedby others. We then used large-sized Lego wheels, because the originally used typewasn’t available any more. These wheels accounted for a large partition of theoverall weight of the robot, but on the other hand allowed us to reach higher topspeeds with almost no critical loss of pushing force.

The gliders were constructed of connector housings glued to Lego pieces that werefastened on the underside of the plane. The connector housings had a surprisinglylow friction on the rink floor and turned out to be way better for our robot thanthe velcro tape suggested in [4]. One glider was fastened in the middle of the frontedge and the other on the middle of the rear edge of the plane .

The battery was placed in the rear of the robot as near to the axis as possiblein order to lower the friction when driving forward. This seemed to be the bestsolution when taking into account that it wasn’t possible to put the battery exactlyabove the axis because of the construction of the upper plane (see 2.2.2). The

11


Figure 2.1.: Schematic drawing of lower plane and casing

battery was held in place with velcro tape.In addition, the lower plane carried two infrared reflex detectors pointing to the

floor. They weren’t mounted on the plane itself but fastened on two wooden beamsextending over the plate in the front left corner and the middle rear, respectively.

Purple Haze’s hockey stick sat to the robot’s right and carried another floor reflexdetector on its tip. The stick consisted of an aluminium beam that was screwedto the acrylic lower plane perpendicular to the wheel axis and therefore pointingstraight forward.

2.2.2. Upper plane

While stiffness was important for the lower plane, the upper plane could be madeof a more lightweight material. We used a very thin acrylic plate that was cut inthe same shape as the lower plane.

12


Figure 2.2.: Puck sensor position and direction

We mounted the controller board on top of the plane together with the LEDdriver board (see 6.2.2) and the main power switch. The main power boostercard sat upside-down on the underside of the plane right above the motors. Thisprevented us from placing the battery on top of the motors, which would have beenthe ideal setup. However, tests showed that this was not a big drawback.

Purple Haze’s IR sensor tower was fastened to the front edge of the plane andwas built from Lego.

We screwed the upper plane to the top ends of the four threaded bolts that wereused for mounting the motors. This allowed us to remove the upper plane whennecessary, e.g. in order to swap batteries.

2.2.3. Casing

The robot’s casing was made of model making cardboard. The big advantages ofthis material are the low weight combined with sufficient stability as well as theeasy processability. The casing was designed to envelop the whole robot, providing

13


a protection to the wheels as recommended by [4]. We fastened the case to thelower plane using only two screws. This allowed us to easily remove the casingwhen necessary.

We mounted another IR sensor on the front of the casing facing approx. 30◦

downwards and hidden under a plastic tube in order to be able to detect puckpossession.

We covered the casing for complexion reasons with a violet coloured artificialfur. This proved later to also have a practical benefit when we realized that the furcushioned the puck, allowing for higher puck approaching speeds.

The casing was exactly the allowed 25cm in diameter [3].

14

3. Sensing

The robot has a total of ten sensors attached - three reflex detectors (floor sensing),five IR-sensors (for puck and navigation) and two slotted opto detectors (motor sens-ing). Sensing is essential for robot construction, since it gives the robot knowledgeabout its world and changes in its environment.

3.1. Reflex Detectors and Floor Sensing

There are three reflex detectors which are placed on the stick, front and rear ofthe robot. We use the sensors to detect when the robot has crossed one of theblackpainted goal zones or the centerline (actually, the robot does not care aboutthe centerline). The sensors deliver a signal between 0 and 255, where a highervalue indicate a darker surface. When one of the situations occur, the strengthof the signal increases and when it reaches a certain threshold value a behaviourresponse will be executed. For this to function, it is mostly a matter of how farfrom the surface the sensors should be placed. If the sensors are placed too far fromthe floor, they will deliver a signal that indicates a very dark surface, therefore itis of great importance to place them very close to the floor. These sensors can beplaced on a distance between 0.5 to 7 millimeters from the surface. For this robotconstruction, we placed them on a distance of about 1 mm for reliable results.

The reflex detectors function for both safety and navigation. The robot needsto know when it has crossed goal yard boundaries to be able to stop or control itsmotion, or else it gets a penalty. When the stick sensor detects a black surface therobot knows that it has reached a goal, but also that it can move a bit further intothe goal yard (to score a goal). If both the stick sensor and the front sensor signalsdetection, the robot has to stop immediately since this means that the robot hascrossed the bound of a goal yard and is on its way to move too far. If the robot

15

3. Sensing

Figure 3.1.: Schematic drawing of the sensor positions on Purple Haze

16

3. Sensing

happens to back into a goal yard the rear sensor functions as a safety mechanismthat makes the robot stop backing any further.

3.2. IR-Sensors - navigation and puck sensing

The robot has four IR-sensors attached to an antenna, used for navigation. Theantenna reaches about 20 cm above the floor and is placed on the front of therobot, to make the sensors able to detect IR-signals from every direction. Thefour IR-sensors are attached to the antenna in a way they can sense IR-signals infour directions and thus cover four quadrants - northwest, northeast, southwest andsoutheast - and are used to detect any objects that emit IR-signals. In this case,such objects are the puck and the two goal yards. By combining the two highestsensor values it is possible to find a direction to an object. The robot calculatesthese values to know where to find the puck and to know where a goal yard is or ifit is close to a goal yard.

The placement of the sensors makes it possible for the robot to correct its move-ment direction when going for a target. If the sensors were placed strictly in thefront, in the back and on the sides of the antenna, only one of them would give astrong signal while the others remain weak. With at least two sensors, it is possibleto combine their values or use the difference between them to improve steering (see4).

For the robot to know when it has a hold on the puck, a fifth sensor is used.The sensor is placed on the front of the robot (like a nose) and is attached in apipe. The reason to use it in a pipe is because we wanted to restrict the incomingsignals from the puck. The result is that the sensor detects the puck only when itis extremely close to the robot. Without the pipe, the robot would act as if it hadthe puck, even though the robot is too far away from it.

3.3. Motor Sensing

For motor sensing two slotted opto detectors are used, which sense the rotation ofthe wheel axis. A slotted opto detector is constructed in a way where the emitter

17

3. Sensing

Figure 3.2.: An encoder disc used in conjunction with the slotted opto detectors

and detector are placed opposite each other (with a gap in between), and signalsdetection when a non-transparent object is between the emitter and the detector.To measure rotation, an encoder disc is placed in the gap between emitter anddetector. The encoder disc has transparent and dark fields (formed like pie slices)placed all around the disc, and is attached to the axis on the wheel side of thegearbox. The sensor is placed in a manner where it has the encoder disc betweenthe emitter and detector. When the axis rotates, the encoder disc rotates andthe sensor detects the transparent and dark fields of the disc. The robot use thisto detect if it is stuck, since as long as the sensor registers changes between theblack and transparent fields the robot is not stuck. If the sensors do not detectany changes within 500 ms and the robot is trying to move, the stuck behaviourbecomes activated.

3.4. Threshold experiments

To make the robot respond to certain sensor signals we used different thresholds.This enables the robot to respond in different ways to a signal from the same source.The only way to find out which thresholds to use for the reflex detectors and theIR-sensors, was to experiment a lot by placing the robot on varying distances fromthe IR emitting objects and also try to change the height of the reflex detectors.During development, continous adjustments of the threshold values were necessarysince variations in ambient lighting caused changes in the obtained readings. The

18

3. Sensing

LCD-display was very useful during the tests.

19

4. Locomotion and manipulation

The main strategy of the robot is to find the puck, go after it and immediatelygo for the opponents goal yard when the puck is caught and then go back to itsdefensive goal. In addition, the robot also has to avoid both goal yards not to moveinto them and get a penalty. The only manipulation the robot performs is whenthe robot tries to move the puck to score.

For locomotion the robot uses two 12 V motors with gearboxes (6.4.1). Sincelocomotion and navigation is based on sensor values, the direction to a certainobject (puck or goal) is basically calculated using the two front sensors mountedon the antenna. The direction value is then used to set the speed of the motors.

direction = sgn(∆IR) ·

(

∆IR

(IRfrontLeft + IRfrontRight ) · T

)

2

where

∆IR = IRfrontLeft − IRfrontRight

The direction value is calculated on the normalized difference between the IR-sensor values in the front (∆IR), and uses a turning speed constant T . Thecalculated value is then subtracted from the left motor speed and added to thespeed of the right motor.

Our steering formula differs from the formulas used by many of the last year’srobots, e.g. winner FejkOpaque [4]. The reason for this is that our robot had avery high turning speed due to the large wheels and lightweight construction. Ittherefore tended to rotate too much before the responsible behaviour was able tocorrect the motor speeds when the target was almost straight ahead. This showedin the form of a swinging movement.

20

4. Locomotion and manipulation

With our square formula, the turning speed is lower when the target is almoststraight ahead and much higher when the target is to the side. The main effectof the square is therefore to damp the turning when the robot is almost exactlylooking towards the target. The signum function, sgn(x), is needed to be able toturn left as well as right, of course.

The robot does not use any kind of feedback control to adapt its speed in differentsituations. The speed is changed dependent of the behaviour responses. The reasonis that the robot was not in need of feedback controlling, since it most of the timewas able to stop within a short distance.

21

5. Software and control

5.1. Development environment

In this course we were given a development environment and compiler for HI-TECHPIC C, a version of C altered a bit to fit our processor. The environment alsoprovided features for debugging and writing the program to the processor usinga programming device called PIC start plus. Actually, the PIC start plus is onlyneeded when the small bootloader program in the PIC has been erased by mistake.Otherwise, one can program the PIC directly in the robot by starting the PIC inprogramming mode and using a serial cable to connect it to the PC. This is ofcourse much easier than moving the PIC from the robot to the programming deviceand back.

However, we had to use the programming device a few times when the boot-loader stopped working. The reason for this is not clear, but we assume that weunintentionally created some short circuit which led to a loss of memory.

5.2. Design overview

We wanted a software design that was easy to maintain and develop. We wantedthe possibility to quickly add a new feature or remove one and not have to rewritethe whole program. We chose a subsumption architecture ([1], [6]).

5.3. Subsumption architecture

The subsumption architecture is based on behaviours. A behaviour is a piece ofcode that decides what it wants the robot to do. When all behaviours have decideda selector selects the behaviour it thinks is best to perform at that given time.

22


Since our robot only has two movable parts (the two wheels), the only thing thebehaviour gives the selector is the speed it wants to set to the two wheels (speedLeftand speedRight) and a bit saying if the behaviour thinks that it really should bedone (active).

5.3.1. Behaviour

Each behaviour can be described as a finite state automaton, where the differ-ent states give different kinds of result. A state transition in a behaviour can betriggered by a sensor stimulus (or several) and/or by time. For example the findpuck behaviour has three states. One that is idle, either because the robot doesnot know where the puck is, the signal is to weak, or because the nose sensor isregistering the puck signal and the robot interprets this as it has the puck. If thereis a strong signal elsewhere than right in front the findPuck behaviour activates.If the puck is somewhere in front it goes to a state that tries to manouvre to thepuck, if the puck is behind the robot the behaviour enters a state where it wantsthe robot to rotate left until the puck is in front.

5.3.2. Behaviour cycle

Since we did not have a real-time operating system for our processor we had tomake the behaviours execute sequentially. To avoid that a behaviour occupies tomuch time, and to avoid inter-behaviour dependencies, we had a few simple rules.

• All (or at least most) common computation is done before all behaviours geta chance to execute.

• A behaviour does not perform heavy computing, no iterative computationsand such.

• A behaviour should only depend on what it self can calculate or what iscalculated by the common computator before all behaviours.

23


5.4. Implementation

The main behaviour loop looks like:

while (true) {

common_computations();

response[0] = behaviour0();

response[1] = behaviour1();

.

.

.

response[n] = behaviourn();

for (i = n; i >= 0; i--)

if (response[i].active)

perform(response[i]);

}

This means that only the behaviour with highest number of those that are activewill control the robot.

5.4.1. The behaviours

We created six different behaviours, here listed in priority order, high priority first(name within brackets):

5 Avoid defensive goal zone [avoidDefGoalZone]

4 Score goal [scoreGoal]

3 Avoid offensive goal zone [avoidOffGoalZone]

2 Handle situations where the robot is stuck [stuck]

1 Find and go to puck [findPuck]

0 Find and goto offensive goal [toOffGoal]

24


Avoid defensive goal zone [avoidDefGoalZone]

This is a very important behaviour, if it does not work our robot will suffer a penalty.This behaviour has three states. The first is active when the defensive goal is faraway, then the robot does not have to watch out for it. The second is the casewhere the goal is close. The third state is when the goal is near and at least one ofthe sensors looking at the floor reacts to something black. If it is the back sensorthe behaviour tells the robot to go forward, if it is one of the two front sensor ittells the robot to go backwards.

Score goal [scoreGoal]

Score goal is inactive and in state zero until the offensive goal is near and in front,and the puck is under the robots control. When that happens a series of statescontrolled by a clock is activated. The robot passes through a backing state, toback away from the puck. A forward and turn state to push the puck, with thestick, further into the goal. When the push state is passed the robot enters a backtowards defensive goal and plays a tune.

Avoid offensive goal zone [avoidOffGoalZone]

This behaviour activates when the offensive goal is near, in front of the robot, andthe stick or front floor sensor is activated. It then wants the robot to go backwards.

+ flo

or se

nsor

activ

e

Goa

l sen

sor h

igh

Goa

l sen

sor l

ow

Floor sensor active

Floor sensor inactive

Goal sensor lowGoal sensor high

In goal zone

Near goal

Idle

Figure 5.1.: The state transitions of both avoid goal zone behaviours

25


Handle situations where the robot is stuck [stuck]

This behaviour has five states. State one is left encoder off, right encoder off. Thesecond is left encoder on, right encoder off etc. The fifth is a state which tries toget the robot out of a stuck situation. It uses five different ways: rotate left, rotateright, back, shove left and shove right. It selects the way at random when it entersthe fifth state, but chooses "‘shove left"’ in 3 out of 7 cases, because this is themost sensible way to recover the puck when stuck at the board. The fifth state isactivated when no state transition has occured in the last second.

Left on, right off Left off, right off

Left off, right onLeft on

Left off

Left on

Left offRig

ht o

n

Rig

ht o

ff

Rig

ht o

n

Rig

ht o

ff

No statechange for one second

Timeout

Retreat

Left on, right on

Figure 5.2.: The stuck behaviour state transitions

Find and go to puck [findPuck]

This behaviour has three states. Idle, when the puck is either not visible or it isright in front of the puck sensor. Turning left to puck, when the puck is behind therobot. Moving to puck, when the puck is in front of the robot. The behaviour usesthe normalized difference between the two forward facing ir-sensors to determinehow to move towards the puck.

Find and goto offensize goal [toOffGoal]

This behaviour is almost exactly like the find puck behaviour but this behaviourchases the offensive goal and it doesn’t stop when the goal is in front of the pucksensor.

26


Puck

in fr

ont o

f rob

ot Puck is behind robot

Puck is in front of robot

Puck

invi

sibl

e or

in c

ontro

lPuck invisible or in control

Puck behind robot

Move to puck

Rotate to puckIdle

Figure 5.3.: The state transitions of find puck behaviour

Goa

l in

fron

t of r

obot G

oal is in front

Goa

l inv

isib

le

Goal invisible

Goal is behind robot

Goal behind robot

Move to goal

Rotate to goalIdle

Figure 5.4.: The state transitions of to offensive goal behaviour

27

6. Electronics

6.1. The main board

The main board has the following devices connected [3]:

• 1 main processor, Microchip Technology PIC16F877

• 1 co-processor, Microchip Technology PIC16F876

• 2 motor drive circuits

• 1 set of dip-switches

• 24 LEDs with series resistors

6.1.1. Main processor PIC16F877

The PIC16F877 microcontroller is used to program the behaviour of the robot andhas the following properties [2]:

• 8K * 14 bits FLASH program memory

• 368 bytes of RAM registers (SRAM)

• 256 bytes of EEPROM non-volatile data memory

• 20 MHz, 5 MIPS RISC

• 40 pin capsule

28

6. Electronics

6.1.2. Co-processor PIC16F876

The PIC16F876 is a 28-pin pre-programmed microcontroller used to filter the signalsfrom a maximum of 5 infrared light sensors. The puck and the goals emitts infraredlight as square waves. These square waves are all different from the sources, andthe objective of the co-processor is to filter the received signals to determine whichsource has been detected. After the signals are filtered, the information is sent tothe main processor.

6.1.3. Motor drive circuits

The motor drive circuits each contain a pair of H-bridges, and can be used to drivefour DC-motors (two 12 V and two 6 V) or speakers. In our case, one of thecircuits is used for two 12 V motors and the other for a 100 ohm speaker. Pulsewidth modulation (PWM) is used to control the average output voltage of themotor drive circuits and thereby the rotational speed of the motors connected.

6.1.4. Dip-switches

The dip-switches main function is to control the direction in which the robot willscore. Except that, we also used one switch as a bootloader control, by connectingthe RC0-pin of the main processor to one of the switches. When the switch wasturned on the RC0-pin became grounded for the bootloader to start, and whenswitched off the RC0 got connected to +5 V for the program to run.

6.2. Auxiliary boards

Three separate boards are used:

• RS-232 signal level converter

• LED driver board

• Voltage booster board

29

6. Electronics

6.2.1. RS-232 signal converter

The board is connected with a DB9 socket, and is used to host a PC through aRS-232 serial cable for program downloading to the main processor. It needs 5 Vpower and is connected to the TX and RX PIC pins.

6.2.2. LED driver board

The LED board supports the reflex detectors and the slotted opto detectors. Weused three reflex detectors and two slotted opto detectors, which were serially con-nected to one of the three pin connectors of the board. The current through theLEDs was adjusted to maximum with the potentiometer on the driver board, forthe slotted opto detectors to work properly.

6.2.3. Voltage booster board

The voltage booster board converts battery voltage 7.2 V to 14.5 V for driving themotors. It gets input power from the battery and outputs converted voltage to themain board and LED driver board.

6.3. Electrical supply

A pack of NiCd batteries (7.2 V) were used during the development of the ro-bot, connected to the voltage booster board. During the competition we switchedbetween using the NiCd batteries and a package of twelve alkaline cells, since wewanted to guarantee the robot was supplied with necessary power.

6.4. Electronic devices and their connections

6.4.1. Motors

We used two 12 V locomotion motors connected to the 12 V motor header pins.They are supplied with pulse width modulated voltage by the motor driver circuits.

30

6. Electronics

6.4.2. Speaker

To make the robot able to play a tune when scoring, we used a 100 ohm speaker.The speaker is connected to one of the 6 V motor headers, and is mounted in theend of a tube for better sound. The volume is controlled by a 1 K ohm trimpot.

6.4.3. Reflex detectors

The reflex detector LEDs are serially connected to the LED driver board. The freeLED+ in one end of the chain is connected to the positive terminal pin on theone of the three pin connectors, while the free LED- is connected to the negativeterminal pin on the LED board. The emitter pin and the collector on each detector isconnected to ground and to a separate analogue input pin (AN0-AN2), respectively.

6.4.4. Slotted opto detectors

The LEDs of the slotted opto detectors are serially connected and are included inthe same chain as the reflex detectors. The emitters are also connected to ground,but instead of connecting each of the collectors to a pin of the A/D-converter, theyare connected to a digital schmitt-triggered pin (RE0 and RE1 and a 4.7k pull-upresistor each) to enable reading in one operation by the program.

6.4.5. IR-sensors

The five IR-sensors are connected via the connectors of the main board to theIR-PIC (the co-processor PIC16F876).

6.4.6. LCD display

The LCD board has a 2x16 character display and an eight pin flat cable which isconnected to the main board. The LCD display has mainly been used for debuggingpurposes and to get information about sensor values as well.

31

7. Discussion

7.1. Results

7.1.1. Construction and sensors

We reached or goal (see also 2.1) to build a lightweighted but at the same timerobust and stiff robot pretty well. The dynamics worked on the whole as expected.

Oscillation problem

Yet, our robot encountered sometimes an oscillation problem when the stick waspositioned over the black goal zone. Since the control software decided in this caseto stop or back the robot, the stick came nearer to the floor which resulted in sensorreadings that were interpreted as white. Thus, the robot did not "see" the goalzone any longer and continued to move, which led to a larger distance between thestick and the floor. The overall result was a funny-to-watch, but counterproductive,vibration of the whole robot. Unfortunately we did not have the time to investigatethis behaviour in detail and develop a "clean" solution, but instead we tried to fixthe problem by changing the sensor threshold value. This turned out to decreasethe oscillation likelihood but did not generally avoid the oscillation. The reason forthis seems to be very large deviations in the sensor readings dependent on the floordistance.

A better solution might have been to completely prevent the stick from nodding,which could have been achieved by using another, stiffer material for the stickand/or changing the glider or sensor setup, respectively.

32

7. Discussion

Maintainability

The robot’s maintainability was good with the medium-sized wheels. We wereable to disassemble the robot quite fast. However, things changed with the needfor mounting the big wheels (see 2.2.1). Since these wheels did not fit throughthe casing when disassembling, we had to remove the wheels first. Therefore, thedisassembling process got quite long winded. Fortunately, there was almost no needto remove the lower plane from the casing.

This was somewhat different with the upper plane. Since the battery sat under it,the plane had to be removed when changing batteries. This meant that all wires hadto be unplugged from the controller board and the IR booster card first and pluggedin again afterwards. Although we tried to have as few connectors as possible, therewere still 9 connectors to be unplugged, including the battery connector. As onecan see, this setup made battery changing quite laborious and turned out to be asource of error because of the risk to unintentionally damage a connection whenplugging in or off. Indeed, we encountered some loose connections during testing,resulting in strange behaviours of the reflex detectors and opto encoders.

In a whole, Purple Haze was very easy to maintain, except for battery changingwich could have been less laborious.

7.1.2. Performance

Our robot performed well. It was able to score goals, both in qualifications and inmatches against other robots.

Our main strength was the speed of our robot. It was noticably faster than allother robots. This gave us the possibility to shove the puck into the goal withoutusing any special stick teknique or shooting mechanism. The drawback was that weneeded to have a clear path from us to the goal that was long enought to accelerate.Our robot could push the puck into the goal using its stick when we did not haveenough speed, but we were not as good at that as some other robots.

One other strenght of our robot was its ability to get out of situations where itwas not able to move the way it liked.

33

A. Source code

A.1. settings.h

1 //PROGRAM SETTINGS

3 // IR s en s o r p o s i t i o n s :4 //−−−−−−−−−−−−−−−−−−−−5 #de f i n e FRONT_LEFT 06 #de f i n e FRONT_RIGHT 17 #de f i n e REAR_LEFT 38 #de f i n e REAR_RIGHT 29 #de f i n e PUCK_SENSOR 4

12 // R e f l e x d e t e c t o r c onn e c t i o n s :13 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−14 #de f i n e REFLEX_STICK 1 //AN115 #de f i n e REFLEX_FRONT 0 //AN016 #de f i n e REFLEX_REAR 2 //AN2

19 // R e f l e x d e t e c t o r t h r e s h o l d s :20 //−−−−−−−−−−−−−−−−−−−−−−−−−−−21 #de f i n e STICK_SENSOR_THRESHOLD 22022 #de f i n e FRONT_SENSOR_THRESHOLD 22023 #de f i n e REAR_SENSOR_THRESHOLD 230

26 //Puck p o s s e s i o n t h r e s h o l d :27 //−−−−−−−−−−−−−−−−−−−−−−−−−28 #de f i n e HAVE_PUCK_THRESHOLD 10

34

A. Source code

31 //Motor speed p r e s e t s :32 //−−−−−−−−−−−−−−−−−−−−33 #de f i n e MAX_SPEED_FORWARD 12734 #de f i n e MAX_SPEED_BACKWARD −12835 #de f i n e MED_SPEED_BACKWARD −10036 #de f i n e MED_SPEED_FORWARD 100

39 //Max . tu rn speed when hunt i ng t a r g e t i n f r o n t o f the robo t :40 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−41 #de f i n e TURN_SPEED 10 // s q r t (max . tu rn speed )

44 // No i se t h r e s h o l d to avo id r e a c t i o n s to ambient l i g h t :45 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−46 #de f i n e NOISE_THRESHOLD 5

49 //Number o f b eha v i o u r s :50 //−−−−−−−−−−−−−−−−−−−−−51 #de f i n e BEHAVIOUR_COUNT 6

54 // Stuck b eha v i o u r :55 //−−−−−−−−−−−−−−−−56 #de f i n e STUCK_TIMEOUT 50057 #de f i n e STUCK_RETREAT_TIME 45058 #de f i n e STUCK_CYCLE_LENGTH 7

61 // s c o r eGoa l b eha v i o u r :62 //−−−−−−−−−−−−−−−−−−−−63 #de f i n e GOAL_SCORING_TURN_TIME 20064 #de f i n e GOAL_SCORING_SPIN_TIME 100065 #de f i n e GOAL_SCORING_RETREAT_TIME 170066 #de f i n e GOAL_SCORING_BACK_TIME 63067 #de f i n e GOAL_SCORING_ATTACK_TIME 80068 #de f i n e GOAL_SCORING_ATTACK_TIME2 150

35

A. Source code

71 // avoidXXXGoalZone b eha v i o u r s :72 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−73 #de f i n e GOAL_ZONE_THRESHOLD 16074 #de f i n e DEF_ZONE_THRESHOLD 140

A.2. main.c

1 #in c l u d e <p i c . h>2 #in c l u d e <con io . h>3 #in c l u d e " d e f i n e s . h"4 #in c l u d e " l c d . h"5 #in c l u d e "adc . h"6 #in c l u d e " s p r i n t . h"7 #in c l u d e "debug . h"8 #in c l u d e "ir_comm . h"9 #in c l u d e "eeprom . h"

10 #in c l u d e "pwm. h"11 #in c l u d e " i n t r . h"

13 // I n c l u d e program s e t t i n g s :14 #in c l u d e " s e t t i n g s . h"

16 //no code p ro t ec t , no WDT, no BOD, power up t imer , HS Xta l17 // ( has no e f f e c t when w r i t i n g the program u s i n g the18 // boot l o a d e r ) :19 __CONFIG(0 x3FB2 ) ;

22 // Me lod i e s f o r s o f twa r e sound g en e r a t o r :23 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−24 #de f i n e TL 9 //Tone l e ng t h

26 // Sta r t−up sound :27 const char l a s e r [ ] = {0 x00 , 2 , 0 x02 , 2 , 0 x04 , 2 , 0 x06 , 2 , 0 x08 , 2 , 0 x0A , 2 , 0

x10 , 2 , 0 x12 , 2 , 0 x14 , 2 , 0 x16 , 2 , 0 x18 , 2 , 0 x1A , 2 , 0 x20 , 2 , 0 x22 , 2 , 0 x24 , 2 , 0x26 , 2 , 0 x28 , 2 , 0 x2A , 2 , 0 x30 , 2 , 0 x32 , 2 , 0 x34 , 2 , 0 x36 , 2 , 0 , 0 } ;

36

A. Source code

29 // Goal s c o r i n g melody ( Pu rp l e Haze ) :30 const char i n t r o [ ] = {0 x2A ,3∗TL, 0 x31 ,3∗TL , 0 x36 ,3∗TL, 0 x28 ,14∗TL, 0 xFF

,1∗TL, 0 x26 ,3∗TL , 0 x11 ,3∗TL , 0 x03 ,3∗TL, 0 x2A ,14∗TL, 0 xFF ,1∗TL , 0 x2A,3∗TL, 0 x31 ,3∗TL , 0 x36 ,3∗TL , 0 x28 ,14∗TL, 0 xFF ,1∗TL, 0 x26 ,3∗TL , 0 x11,3∗TL, 0 x03 ,3∗TL , 0 x2A ,14∗TL , 0 xFF ,1∗TL , 0 , 0 } ;

33 // Bas i c math macros :34 //−−−−−−−−−−−−−−−−−−35 #de f i n e MAX(x , y ) ( ( x<y ) ? y : x )36 #de f i n e MIN(x , y ) ( ( x>y ) ? y : x )37 #de f i n e ABS( x ) ( ( x>0) ? x : − x )

40 // S t r u c t f o r b eha v i o u r r e s p on s e encod ing :41 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−42 t ypede f s t r u c t {43 s i gned char l e f t ; // Speed o f LEFT motor44 s i gned char r i g h t ; // Speed o f RIGHT motor45 uns igned char a c t i v e ; //" Behav iour ACTIVE"− f l a g46 } Behav iour ;

49 // G l oba l v a r i a b l e s :50 //−−−−−−−−−−−−−−−−−51 // Bu f f e r f o r LCD d i s p l a y :52 bank1 s t a t i c char b u f f e r [ 7 ] ;

54 // Momentar i l y a p p l i e d speed s to l e f t / r i g h t motor :55 bank2 s t a t i c s i gned char s p e edLe f t = 0 ;56 bank2 s t a t i c s i gned char speedR igh t = 0 ;

58 // IR r e a d i n g s ( puck ) :59 bank2 s t a t i c uns igned i n t i rPuck [ 5 ] ;60 bank2 s t a t i c uns igned i n t i rPuckSum ;

62 // IR r e a d i n g s ( o f f e n s i v e goa l ) . Only 4 , because puck s en s o r noti n v o l v e d :

63 bank2 s t a t i c uns igned i n t i r O f f [ 4 ] ;64 bank2 s t a t i c uns igned i n t i rOf fSum ;

37

A. Source code

66 // IR r e a d i n g s ( d e f e n s i v e goa l ) . Only 4 , because puck s en s o r noti n v o l v e d :

67 bank2 s t a t i c uns igned i n t i r D e f [ 4 ] ;68 bank2 s t a t i c uns igned i n t i rDefSum ;

70 // Sensor r e ad i n g ( s t i c k r e f l e x s e n s o r ) :71 bank2 s t a t i c uns igned char r e f l e x S t i c k = 0 ;

73 // R e f l e x s e n s o r s above b l a c k ground?74 bank2 s t a t i c b i t s t i c k S e n s o r = 0 ;75 bank2 s t a t i c b i t f r o n t S e n s o r = 0 ;76 bank2 s t a t i c b i t backSensor = 0 ;

78 //Puck p o s s e s s i o n :79 bank2 s t a t i c b i t hasPuck = 0 ;

81 // Encoder d i s c f l a g s :82 bank2 s t a t i c b i t e n cL e f t = 0 ;83 bank2 s t a t i c b i t encR igh t = 0 ;

85 //Robot near o f f e n s i v e / d e f e n s i v e goa l ?86 bank2 s t a t i c b i t goa lZone = 0 ;87 bank2 s t a t i c b i t defZone = 0 ;

92 s i gned char l i m i t S p e e d ( s i gned i n t speed ) {93 // L im i t s " speed " to the v a l i d range −128 . .12794 i f ( speed > MAX_SPEED_FORWARD) speed = MAX_SPEED_FORWARD;95 i f ( speed < MAX_SPEED_BACKWARD) speed = MAX_SPEED_BACKWARD;96 r e t u r n ( s i gned char ) speed ;97 }

100 Behav iour f i n d d i r ( bank2 uns igned i n t ∗ i r , s i gned char baseSpeed ) {101 // Thi s f u n c t i o n r e t u r n s whee l speed s tha t make the robo t102 // d r i v e towards the t a r g e t g i v e n by the " i r " p o i n t e r .103 //" baseSpeed " i s used to chose the o v e r a l l d r i v i n g speed ,

38

A. Source code

104 // 0 means " r o t a t e i n p l a c e " .105 // Thi s c a l c u l a t i o n i s not c o r r e c t i n a l l cases , s i n c e v a l i d106 // speed s must be w i t h i n −128 . . 1 2 7 . Works on l y i f t a r g e t i s i n107 // f r o n t , o t h e rw i s e no s e n s i b l e r e s u l t s !108 s i gned i n t d i r = 0 ;109 uns igned i n t sumFront = 0 ;110 Behav iour r e s ;

112 r e s . l e f t = r e s . r i g h t = baseSpeed ;

114 sumFront = i r [FRONT_LEFT] + i r [FRONT_RIGHT ] ;

116 // Ca l c u l a t e the no rma l i z ed d i f f e r e n c e between117 // the l e f t and r i g h t i r s e n s o r :118 d i r = i r [FRONT_LEFT] − i r [FRONT_RIGHT ] ;119 d i r = d i r / ( ( s i gned i n t ) ( sumFront / TURN_SPEED + 1) ) ;

121 //Take the squa r e o f the no rma l i z ed d i f f e r e n c e i n o r d e r122 // to r o t a t e s l owe r i f the t a r g e t i s n e a r l y i n f r o n t ,123 // and f a s t e r i f i t i s to the s i d e . Keep s i g n :124 d i r = ( d i r < 0) ? −( d i r ∗ d i r ) : ( d i r ∗ d i r ) ;

126 // L im i t r e s u l t s to v a l i d speed range :127 r e s . l e f t = l im i t S p e e d ( r e s . l e f t − d i r ) ;128 r e s . r i g h t = l im i t S p e e d ( r e s . r i g h t + d i r ) ;

130 r e t u r n r e s ;131 }

134 Behav iour f i ndPuck ( ) {135 // Thi s b eha v i o u r makes the robo t head towards the puck136 bank3 s t a t i c uns igned char s t a t e = 0 ;137 uns igned i n t f rontSum = i rPuck [FRONT_LEFT] + i rPuck [FRONT_RIGHT ] ;138 uns igned i n t backSum = i rPuck [REAR_LEFT] + i rPuck [REAR_RIGHT ] ;139 uns igned i n t l e f tSum = i rPuck [FRONT_LEFT] + i rPuck [REAR_LEFT ] ;140 uns igned i n t r ightSum = i rPuck [FRONT_RIGHT] + i rPuck [REAR_RIGHT ] ;141 Behav iour r e s ;

143 r e s . a c t i v e = 0 ;

39

A. Source code

145 // S t r u c t u r e o f the FSA :146 //−−−−−−−−−−−−−−−−−−−−−147 sw i t ch ( s t a t e ) {

149 //STATE 0 : Puck i n v i s i b l e o r r obo t has puck150 case 0 : {151 i f ( irPuckSum > NOISE_THRESHOLD && ! hasPuck ) {152 i f ( frontSum>backSum && frontSum>le f tSum && frontSum>

rightSum )153 s t a t e = 1 ;154 e l s e

155 s t a t e = 2 ;156 }157 break ;158 }

160 //STATE 1 : Puck i s i n f r o n t o f the robo t161 case 1 : {162 i f ( irPuckSum < NOISE_THRESHOLD | | hasPuck )163 s t a t e = 0 ;164 i f ( f rontSum < backSum && frontSum < le f tSum && frontSum <

rightSum )165 s t a t e = 2 ;166 break ;167 }

169 //STATE 2 : Puck i s i n the back o f the robo t170 case 2 : {171 i f ( irPuckSum < NOISE_THRESHOLD)172 s t a t e = 0 ;173 i f ( f rontSum > backSum && frontSum > le f tSum && frontSum >

rightSum )174 s t a t e = 1 ;175 break ;176 }177 }

179 // Act i on s o f the FSA :180 //−−−−−−−−−−−−−−−−−−−

40

A. Source code

181 sw i t ch ( s t a t e ) {

183 case 0 : { break ; }

185 case 1 : {186 r e s = f i n d d i r ( i rPuck , MAX_SPEED_FORWARD) ;187 r e s . a c t i v e = 1 ;188 break ;189 }

191 case 2 : {192 // Rotate i n p l a c e :193 r e s . l e f t = MAX_SPEED_BACKWARD;194 r e s . r i g h t = MAX_SPEED_FORWARD;195 r e s . a c t i v e = 1 ;196 break ;197 }198 }

200 r e t u r n r e s ;201 }

204 Behav iour toOf fGoa l ( ) {205 // Thi s b eha v i o u r makes the robo t head towards the OFFENSIVE goa l206 // w i thou t r e g a r d to puck p o s s e s i o n .207 bank3 s t a t i c uns igned char s t a t e = 0 ;208 uns igned i n t f rontSum = i r O f f [FRONT_LEFT] + i r O f f [FRONT_RIGHT ] ;209 uns igned i n t backSum = i r O f f [REAR_LEFT] + i r O f f [REAR_RIGHT ] ;210 uns igned i n t l e f tSum = i r O f f [FRONT_LEFT] + i r O f f [REAR_LEFT ] ;211 uns igned i n t r ightSum = i r O f f [FRONT_RIGHT] + i r O f f [REAR_RIGHT ] ;212 Behav iour r e s ;

214 r e s . a c t i v e = 0 ;


220 //STATE 0 : Goal i n v i s i b l e

41

A. Source code

221 case 0 : {222 i f ( i rOf fSum > NOISE_THRESHOLD) {223 i f ( frontSum>backSum && frontSum>le f tSum && frontSum>

rightSum )224 s t a t e = 1 ;225 e l s e

226 s t a t e = 2 ;227 }228 break ;229 }

231 //STATE 1 : Goal i n f r o n t o f the robo t232 case 1 : {233 i f ( i rOf fSum < NOISE_THRESHOLD)234 s t a t e = 0 ;235 i f ( ! ( frontSum>backSum && frontSum>le f tSum && frontSum>

rightSum ) )236 s t a t e = 2 ;237 break ;238 }

240 //STATE 2 : Goal i n the back o f the robo t241 case 2 : {242 i f ( i rOf fSum < NOISE_THRESHOLD)243 s t a t e = 0 ;244 i f ( f rontSum > backSum && frontSum > le f tSum && frontSum >

rightSum )245 s t a t e = 1 ;246 break ;247 }248 }

250 // Act i on s o f the FSA :251 //−−−−−−−−−−−−−−−−−−−252 sw i t ch ( s t a t e ) {

254 case 0 : { break ; }

256 case 1 : {257 r e s = f i n d d i r ( i rO f f , MAX_SPEED_FORWARD) ;

42

A. Source code

258 r e s . a c t i v e = 1 ;259 break ;260 }

262 case 2 : {263 // Rotate i n p l a c e :264 r e s . l e f t = MED_SPEED_BACKWARD;265 r e s . r i g h t = MAX_SPEED_FORWARD;266 r e s . a c t i v e = 1 ;267 }268 }

270 r e t u r n r e s ;271 }

274 Behav iour s tuck ( ) {275 // Thi s b eha v i o u r t r i e s to f i g u r e out i f the robo t i s s tuck276 // and i f i t t h i n k s the robo t i s , choo se s randomly a back ing277 // s t r a t e g y .278 bank3 s t a t i c uns igned char s t a t e = 0 ;279 bank3 s t a t i c uns igned i n t r e t r e a tT ime = 0 ;280 bank3 s t a t i c uns igned i n t stuckTime = 0 ;281 bank3 s t a t i c uns igned char d i r e c t i o n = 0 ;282 Behav iour r e s ;

284 r e s . a c t i v e = 0 ;

286 i f ( s t a t e !=4 && pas s ed ( stuckTime + STUCK_TIMEOUT) && spe edLe f t!=0 && speedR igh t !=0) {

287 // Enter the s tuck hand l i n g s t a t e :288 r e t r e a tT ime = sof t_t ime ( ) ;289 d i r e c t i o n = ( r e f l e x S t i c k + i rOf fSum ) % STUCK_CYCLE_LENGTH;290 s t a t e = 4 ;291 r e t u r n r e s ;292 }


43

A. Source code

298 //STATE 0 −3: A l l p o s s i b l e comb ina t i on s o f the two encode r s299 case 0 : {300 i f ( e n cL e f t ) {301 s t a t e = 1 ;302 stuckTime = sof t_t ime ( ) ;303 }304 e l s e i f ( encR igh t ) {305 s t a t e = 2 ;306 stuckTime = sof t_t ime ( ) ;307 }308 break ;309 }

311 case 1 : {312 i f ( ! e n cL e f t ) {313 s t a t e = 0 ;314 stuckTime = sof t_t ime ( ) ;315 }316 e l s e i f ( encR igh t ) {317 s t a t e = 3 ;318 stuckTime = sof t_t ime ( ) ;319 }320 break ;321 }

323 case 2 : {324 i f ( e n cL e f t ) {325 s t a t e = 3 ;326 stuckTime = sof t_t ime ( ) ;327 }328 e l s e i f ( ! encR igh t ) {329 s t a t e = 0 ;330 stuckTime = sof t_t ime ( ) ;331 }332 break ;333 }

335 case 3 : {336 i f ( ! e n cL e f t ) {

44

A. Source code

337 s t a t e = 2 ;338 stuckTime = sof t_t ime ( ) ;339 }340 e l s e i f ( ! encR igh t ) {341 s t a t e = 1 ;342 stuckTime = sof t_t ime ( ) ;343 }344 break ;345 }

347 //STATE 4 : T r i g g e r back ing s t r a t e g y348 case 4 : {349 i f ( pa s s ed ( r e t r e a tT ime + STUCK_RETREAT_TIME) )350 s t a t e = 0 ;351 }352 }


358 case 0 : { break ; }359 case 1 : { break ; }360 case 2 : { break ; }361 case 3 : { break ; }

363 case 4 : {364 sw i t ch ( d i r e c t i o n ) {365 // D i f f e r e n t back ing s t r a t e g i e s .

367 //0−2 a r e the same to i n c r e a s e p o s s i b i l i t y368 // f o r t h i s back ing s t r a t e g y369 case 0 : {370 r e s . l e f t = MAX_SPEED_BACKWARD;371 r e s . r i g h t = 0 ;372 break ;373 }374 case 1 : {375 r e s . l e f t = MAX_SPEED_BACKWARD;376 r e s . r i g h t = 0 ;

45

A. Source code

377 break ;378 }379 case 2 : {380 r e s . l e f t = MAX_SPEED_BACKWARD;381 r e s . r i g h t = 0 ;382 break ;383 }384 case 3 : {385 r e s . l e f t = MAX_SPEED_BACKWARD;386 r e s . r i g h t = MAX_SPEED_FORWARD;387 break ;388 }389 case 4 : {390 r e s . l e f t = MAX_SPEED_FORWARD;391 r e s . r i g h t = MAX_SPEED_BACKWARD;392 break ;393 }394 case 5 : {395 r e s . l e f t = MAX_SPEED_BACKWARD;396 r e s . r i g h t = MAX_SPEED_BACKWARD;397 break ;398 }399 case 6 : {400 r e s . l e f t = 0 ;401 r e s . r i g h t = MAX_SPEED_BACKWARD;402 break ;403 }404 }405 r e s . a c t i v e = 1 ;406 stuckTime = sof t_t ime ( ) ;407 }408 }

410 r e t u r n r e s ;411 }

415 Behav iour s c o r eGoa l ( ) {416 // Thi s b eha v i o u r t r i e s to s c o r e .

46

A. Source code

417 bank3 s t a t i c uns igned char s t a t e = 0 ;418 bank3 s t a t i c uns igned i n t s t a r tT ime = 0 ;419 Behav iour r e s ;420 r e s . a c t i v e = 0 ;


426 //STATE 0 : Robot doesn ’ t have the puck or i s n ’ t i n goa l zoneye t

427 case 0 : {428 i f ( hasPuck && s t i c k S e n s o r && goa lZone ) {429 s t a t e = 1 ;430 s t a r tT ime = sof t_t ime ( ) ;431 }432 break ;433 }

435 //STATE 1 : Dr i v e backwards436 case 1 : {437 i f ( pa s s ed ( s t a r tT ime + GOAL_SCORING_BACK_TIME) ) {438 s t a r tT ime = sof t_t ime ( ) ;439 s t a t e = 2 ;440 }441 break ;442 }

444 //STATE 2 : Try to poke the puck i n445 case 2 : {446 i f ( pa s s ed ( s t a r tT ime + GOAL_SCORING_ATTACK_TIME) ) {447 s t a r tT ime = sof t_t ime ( ) ;448 s t a t e = 4 ;449 play_tune ( i n t r o ) ; // Goal−attempt melody450 }451 i f ( s t i c k S e n s o r ) {452 s t a r tT ime = sof t_t ime ( ) ;453 s t a t e = 3 ;454 }455 break ;

47

A. Source code

456 }

458 //STATE 3 : Cont inue pok ing movement even though459 // the s t i c k i s a l r e a d y i n the goa l zone460 case 3 : {461 i f ( pa s s ed ( s t a r tT ime + GOAL_SCORING_ATTACK_TIME2) ) {462 s t a r tT ime = sof t_t ime ( ) ;463 s t a t e = 4 ;464 play_tune ( i n t r o ) ; // Goal−attempt melody465 }466 break ;467 }

469 //STATE 4 : Dr i v e towards the home goa l470 case 4 : {471 i f ( pa s s ed ( s t a r tT ime + GOAL_SCORING_RETREAT_TIME) | | defZone )472 s t a t e = 0 ;473 break ;474 }475 }


481 case 0 : {482 r e s . a c t i v e = 0 ;483 break ;484 }

486 case 1 : {487 r e s . l e f t = MED_SPEED_BACKWARD;488 r e s . r i g h t = MED_SPEED_BACKWARD;489 r e s . a c t i v e = 1 ;490 break ;491 }

493 case 2 : {494 r e s . l e f t = MED_SPEED_FORWARD;495 r e s . r i g h t = MAX_SPEED_FORWARD;

48

A. Source code

496 r e s . a c t i v e = 1 ;497 break ;498 }

500 case 3 : {501 r e s . l e f t = MED_SPEED_FORWARD;502 r e s . r i g h t = MAX_SPEED_FORWARD;503 r e s . a c t i v e = 1 ;504 break ;505 }

507 case 4 : {508 //" Missuse " the f i n d d i r f u n c t i o n to head backwards .509 // S imply take the two r e a r IR s e n s o r s as " f r o n t s e n s o r s "510 //and s e t the baseSpeed to MAX_SPEED_BACKWARD.511 r e s = f i n d d i r (& i rD e f [REAR_RIGHT] , MAX_SPEED_BACKWARD) ;512 r e s . a c t i v e = 1 ;513 break ;514 }515 }

517 r e t u r n r e s ;518 }

522 Behav iour avo idOf fGoa lZone ( ) {523 // Thi s b eha v i o u r p r e v en t s the robo t from en t e r i n g the o f f e n s i v e524 // goa l zone . Although e n t e r i n g i s n ’ t p r o h i b i t e d by the r u l e s , t h i s525 // b eha v i o u r i s n e c e s s a r y to be ab l e to s c o r e .526 bank3 s t a t i c uns igned char s t a t e = 0 ;527 Behav iour r e s ;

529 r e s . a c t i v e = 0 ;


535 //STATE 0 : Not i n goa l zone

49

A. Source code

536 case 0 : {537 i f ( goa lZone && ( s t i c k S e n s o r | | f r o n t S e n s o r ) )538 s t a t e = 1 ;539 break ;540 }

542 //STATE 1 : S t i c k or f r o n t i s i n goa l zone543 case 1 : {544 i f ( ! s t i c k S e n s o r && ! f r o n t S e n s o r )545 s t a t e = 0 ;546 break ;547 }548 }


555 case 0 : { break ; }

557 case 1 : {558 // Dr i v e backwards wi th f u l l speed :559 r e s . l e f t = MAX_SPEED_BACKWARD;560 r e s . r i g h t = MAX_SPEED_BACKWARD;561 r e s . a c t i v e = 1 ;562 break ;563 }564 }

566 r e t u r n r e s ;567 }

571 Behav iour avo idDefGoa lZone ( ) {572 // Thi s b eha v i o u r p r e v en t s the robo t from en t e r i n g i t s own goa l zone

.573 bank3 s t a t i c uns igned char s t a t e = 0 ;574 Behav iour r e s ;

50

A. Source code

576 r e s . a c t i v e = 0 ;


582 //STATE 0 : Not i n d e f e n s i v e goa l zone583 case 0 : {584 i f ( defZone && ( s t i c k S e n s o r | | f r o n t S e n s o r ) )585 s t a t e = 1 ;586 e l s e i f ( defZone && backSensor )587 s t a t e = 2 ;588 break ;589 }

591 //STATE 1 : Robot i n d e f e n s i v e goa l zone , f r o n t towards goa l592 case 1 : {593 i f ( ! s t i c k S e n s o r && ! f r o n t S e n s o r )594 s t a t e = 0 ;595 break ;596 }

598 //STATE 2 : Robot i n d e f e n s i v e goa l zone , back towards goa l599 case 2 : {600 i f ( ! backSensor )601 s t a t e = 0 ;602 break ;603 }604 }


611 case 0 : { break ; }

613 case 1 : {614 // Dr i v e backwards wi th f u l l speed :

51

A. Source code

615 r e s . l e f t = MAX_SPEED_BACKWARD;616 r e s . r i g h t = MAX_SPEED_BACKWARD;617 r e s . a c t i v e = 1 ;618 break ;619 }

621 case 2 : {622 // Dr i v e f o rwa rd wi th f u l l speed :623 r e s . l e f t = MAX_SPEED_FORWARD;624 r e s . r i g h t = MAX_SPEED_FORWARD;625 r e s . a c t i v e = 1 ;626 break ;627 }628 }

630 r e t u r n r e s ;631 }

635 vo id per fo rm ( Behav iour ∗ r e a c t i o n ) {636 // Le t s the b eha v i o u r w i th the h i g h e s t p r i o r i t y per fo rm637 // i t s d e s i r e d a c t i o n . Pu r e l y c ompe t i t i v e c o o r d i n a t i o n −

638 // subsumpt ion a r c h i t e c t u r e !639 //640 // h i g h e r i n d e x i n " b eha v i o u r " a r r a y <=> h i gh e r p r i o r i t y641 //642 //number o f b eha v i o u r s i n s e t t i n g s . h !643 s i gned char i ;

645 f o r ( i = BEHAVIOUR_COUNT − 1 ; i >= 0; i −−)646 i f ( r e a c t i o n [ i ] . a c t i v e ) {647 s p e edLe f t = r e a c t i o n [ i ] . l e f t ;648 speedR igh t = r e a c t i o n [ i ] . r i g h t ;649 break ;650 }

652 // P r i n t i n f o rma t i o n on a c t i v e b eha v i o u r653 s p r i n t 1 6 ( bu f f e r , i ) ; //Number o f a c t i v e b eha v i o u r654 l c d_p r i n t 1 ( "ACT : " , 9 ) ;

52

A. Source code

655 l c d_pr i n t_at_cur so r ( b u f f e r ) ;656 l c d_pr i n t_at_cur so r ( " " ) ;

658 // Set motor speed :659 motors ( speedLe f t , speedR igh t ) ;660 }

664 vo id b a s i c _ i n i t s ( ) {665 // Thi s f u n c t i o n pe r f o rms some s t a r t −up i n i t i a l i s a t i o n666 // t a s k s .667 uns igned i n t sometime ;

669 //Use 3 A/D channe l s (AN0 , AN1 & AN2)670 ADCON1 = 0 b00000010 ;671 TRISA0 = TRISA1 = TRISA2 = 1 ;

673 i r _ i n i t ( ) ; // I n i t i a l i s e IR co−p r o c e s s o r communicat ion674 init_pwm ( ) ; // I n i t i a l i s e pwm f o r motor c o n t r o l675 i n i t_so f t_tmr ( ) ; // I n i t i a l i s e s o f t t ime r676 i n i t_sound ( ) ; // I n i t i a l i s e sound g en e r a t o r677 i n i t_en c ( ) ;

679 // Enab le i n t e r r r u p t s :680 PEIE = 1 ; // P e r i p h e r a l i n t e r r u p t s e n ab l e681 e i ( ) ; // G l oba l i n t e r r u p t e nab l e

683 //Turn on LCD:684 set_lcd_power (1 ) ;

686 //Wait f o r 100ms . . .687 sometime = sof t_t ime ( ) ;688 f o r ( ; ; )689 i f ( pa s s ed ( sometime + 100) )690 break ;

692 // . . . and then :693 i n i t_ l c d ( ) ;

53

A. Source code

695 // Play ’ l a s e r ’ sound at s t a r t u p :696 play_tune ( l a s e r ) ;697 }

701 vo id ge tVa lue s ( ) {702 //Read a l l s e n s o r v a l u e s and compute r e l e v a n t data703 // and f l a g s .704 uns igned i n t puckSensor = 0 ;

706 // Synch ron i z e to IR r e c e p t i o n :707 i r_wa i t ( ) ;

709 // O f f e n s i v e goa l ( no puck s en s o r ! ) :710 i r O f f [FRONT_LEFT] = i r_va l u e (FRONT_LEFT, OFF_GOAL) ;711 i r O f f [FRONT_RIGHT] = i r_va l u e (FRONT_RIGHT, OFF_GOAL) ;712 i r O f f [REAR_LEFT] = i r_va l u e (REAR_LEFT, OFF_GOAL) ;713 i r O f f [REAR_RIGHT] = i r_va l u e (REAR_RIGHT , OFF_GOAL) ;714 i rOf fSum = i r O f f [FRONT_LEFT] + i r O f f [FRONT_RIGHT] + i r O f f [

REAR_LEFT] + i r O f f [REAR_RIGHT ] ;

716 // De f e n s i v e goa l ( no puck s en s o r ! ) :717 i r D e f [FRONT_LEFT] = i r_va l u e (FRONT_LEFT, DEF_GOAL) ;718 i r D e f [FRONT_RIGHT] = i r_va l u e (FRONT_RIGHT, DEF_GOAL) ;719 i r D e f [REAR_LEFT] = i r_va l u e (REAR_LEFT, DEF_GOAL) ;720 i r D e f [REAR_RIGHT] = i r_va l u e (REAR_RIGHT , DEF_GOAL) ;721 i rDefSum = i rD e f [FRONT_LEFT] + i rD e f [FRONT_RIGHT] + i rD e f [

REAR_LEFT] + i rD e f [REAR_RIGHT ] ;

723 //Puck :724 i rPuck [FRONT_LEFT] = i r_va l u e (FRONT_LEFT, PUCK) ;725 i rPuck [FRONT_RIGHT] = i r_va l u e (FRONT_RIGHT, PUCK) ;726 i rPuck [REAR_LEFT ] = i r_va l u e (REAR_LEFT, PUCK) ;727 i rPuck [REAR_RIGHT] = i r_va l u e (REAR_RIGHT , PUCK) ;728 i rPuck [PUCK_SENSOR] = i r_va l u e (PUCK_SENSOR, PUCK) ;729 i rPuckSum = i rPuck [FRONT_LEFT] + i rPuck [FRONT_RIGHT] + i rPuck [

REAR_LEFT] + i rPuck [REAR_RIGHT ] ;

731 // R e f l e x d e t e c t o r s :

54

A. Source code

732 r e f l e x S t i c k = sample_ad_channel (REFLEX_STICK) ;733 s t i c k S e n s o r = ( r e f l e x S t i c k > STICK_SENSOR_THRESHOLD) ;734 f r o n t S e n s o r = ( sample_ad_channel (REFLEX_FRONT) >

FRONT_SENSOR_THRESHOLD) ;735 backSensor = ( sample_ad_channel (REFLEX_REAR) >

REAR_SENSOR_THRESHOLD) ;

737 // Encoders :738 en cL e f t = ! (RE0 == 0) ;739 encR igh t = ! (RE1 == 0) ;

741 //Puck p o s s e s i o n ( use no rma l i z ed puck s en s o r r e ad i n g ) :742 puckSensor = i rPuck [PUCK_SENSOR] / ( ( i rPuck [FRONT_LEFT] + i rPuck [

FRONT_RIGHT] + i rPuck [PUCK_SENSOR] ) / 100 + 1) ;743 hasPuck = ( puckSensor > HAVE_PUCK_THRESHOLD) ;

745 // Goal zone d e t e c t i o n :746 // O f f e n s i v e goa l :747 goa lZone = ( i r O f f [FRONT_LEFT] + i r O f f [FRONT_RIGHT] >

GOAL_ZONE_THRESHOLD) ;

749 // De f e n s i v e goa l :750 defZone = ( irDefSum > DEF_ZONE_THRESHOLD) ;

753 // P r i n t f l a g s :754 l c d_p r i n t 1 ( ( s t i c k S e n s o r ) ? "S" : "−" , 0 ) ;755 l c d_p r i n t 1 ( ( f r o n t S e n s o r ) ? "F" : "−" , 1 ) ;756 l c d_p r i n t 1 ( ( backSensor ) ? "B" : "−" , 2 ) ;757 l c d_p r i n t 1 ( ( hasPuck ) ? "P" : "−" , 3 ) ;758 l c d_p r i n t 1 ( ( goa lZone ) ? "G" : "−" , 4 ) ;759 l c d_p r i n t 1 ( ( defZone ) ? "D" : "−" , 5 ) ;760 l c d_p r i n t 1 ( ( e n cL e f t ) ? "L" : "o" , 6 ) ;761 l c d_p r i n t 1 ( ( encR igh t ) ? "R" : "o" , 7 ) ;762 }

766 main ( ) {767 Behav iour r e a c t i o n [BEHAVIOUR_COUNT] ; // Behav iour r e s p on s e s

55

A. Source code

769 b a s i c _ i n i t s ( ) ;

771 //MAIN LOOP772 f o r ( ; ; ) {773 ge tVa lue s ( ) ;

775 r e a c t i o n [ 0 ] = toOf fGoa l ( ) ;776 r e a c t i o n [ 1 ] = f i ndPuck ( ) ;777 r e a c t i o n [ 2 ] = s tuck ( ) ;778 r e a c t i o n [ 3 ] = avo idOf fGoa lZone ( ) ;779 r e a c t i o n [ 4 ] = s c o r eGoa l ( ) ;780 r e a c t i o n [ 5 ] = avo idDefGoa lZone ( ) ;

782 per fo rm ( r e a c t i o n ) ;783 }784 }

A.3. intr.h

1 // ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ i n t r . h

4 ex te rn b i t has_passed ;5 ex te rn uns igned i n t soft_tmr ;6 ex te rn uns igned i n t soft_tmr_copy ;

8 // Macro to read the s o f t t ime r a t om i c a l l y9 // Use on l y t h i s macro to read the t ime o u t s i d e the i n t e r r u p t

r o u t i n e10 // USE pas s ed ( time_to_check) MACRO INSTEAD FOR COMPARISONS ! ! !11 #de f i n e so f t_t ime ( ) ( d i ( ) , soft_tmr_copy=soft_tmr , e i ( ) , soft_tmr_copy

)

13 // Macro to check i f a t ime i s i n the pa s t ( or p r e s e n t ) a t om i c a l l y14 // Use on l y t h i s macro to compare to c u r r e n t t ime ( o u t s i d e the15 // i n t e r r u p t r o u t i n e ) .

56

A. Source code

16 // Returns 1 i f t ime_to_check has been pas s ed . Because the t imeva r s

17 // a r e on l y 1 6 b i t s i t must then r e t u r n 0 aga in a f t e r some amounto f

18 // t ime he r e chosen to be 1024 ms .19 // Maximum t ime f o r d e l a y s i s then 65535ms−1024ms or about 6 4 . 5 s e c

.20 #de f i n e pas s ed ( time_to_check) \21 ( d i ( ) , has_passed=(( uns igned i n t ) ( soft_tmr −(time_to_check) ) <1024) ,

e i ( ) , has_passed )

23 // Use above macros l i k e t h i s :24 // sometime=sof t_t ime ( ) ;25 // f o r ( ; ; ) {26 // i f ( pa s s ed ( sometime +60000) ) break ;27 // }28 //29 // The f o r l oop w i l l b reak a f t e r 6 0 s econds30 // ( 6 4 . 5 s e conds max as s t a t e d above )31 // Of c ou r s e yhe program would no rma l l y do o t h e r32 // t h i n g s than j u s t e x e cu t i n g a f o r l oop wh i l e wa i t i n g

35 // i n i t i a l i s e the s o f t t ime r36 // use s TIMER137 ex te rn vo id i n i t_so f t_tmr ( ) ;

40 // i n i t i a l i s e tone g en e r a t o r41 // use s TIMER042 ex te rn vo id i n i t_sound ( ) ;

44 // s t a r t p l a y i n g a tune45 // ( non−b l o c k i n g )46 // tune i s an a r r a y o f c h a r s i n the f o l l o w i n g fo rmat :47 // { note , t ime , note , t ime , . . . . , 0 , 0 }48 // i . e . i t c o n t a i n s no t e s and t h e i r dua r t i on , and a lway s49 // ends wi th two z e r o s50 // format o f note :51 // b i t s 0−3 i s the number o f h a l f t one s from the base tone i n each

57

A. Source code

oc tave52 // ( so tha t i f C was the base tone i t would be 0 , D=2 , E=4 , F=5 , G

=7,53 // A=9 , and H=11; c f coun t i ng the key s o f a piano , i n c l u d i n g the

b l a c k ones )54 // b i t s 4−5 i s the oc tave number from 0 to 355 // i f b i t s 4−7 a r e a l l 1 the note w i l l be qu i e t , i . e . a pause56 // format o f t ime :57 // du r a t i o n o f note i n u n i t s o f 8 ms , so tha t 1 s i s 1 2 5 u n i t s .58 // Two equa l no t e s ( not n e c e s s a r i l y w i th the same du r a t i o n )59 // w i l l sound l i k e merged to one l o n g e r note . I n s e r t a sma l l pause60 // to avo id t h i s ( and s u b t r a c t the t ime o f the pause ( one u n i t ?)61 // from tha t o f the f i r s t o f the equa l no t e s62 //63 // a c a l l w i l l abo r t any a l r e a d y p l a y i n g tune and s t a r t p l a y i n g the

new tune64 // ( i . e . p l a y i n g the empty tune {0 , 0 } w i l l j u s t abo r t p r e v i o u s

tune s )65 ex te rn vo id play_tune ( const char ∗ tune ) ;

69 // read the encode r p o s i t i o n s from th e s e v a r s70 // ( can a l s o c l e a r e d from main program l i k e " enc1_pos =0;")71 ex te rn uns igned char enc1_pos , enc2_pos ;

73 // e r r o r b i t s tha t a r e s e t by i n t e r r u p t i f a p u l s e has been missed74 ex te rn b i t enc1_err ;75 ex te rn b i t enc2_err ;

77 // i n i t f u n c t i o n to c a l l b e f o r e u s i n g encode r s78 ex te rn vo id i n i t_en c ( ) ;

A.4. pwm.h

1 // ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ pwm. h

58

A. Source code

3 // i n i t i a l i s e PWM4 // use s both CCP p i n s ( RC1 and RC2 ; connect t h e s e to enb l p i n s5 // near the motor d r i v e r c i r c u i t ) as w e l l a s6 // the two motor d i r e c t i o n c o n t r o l p i n s d e f i n e d i n pwm. c ( connect7 // t h e s e to d i r p i n s near the motor d r i v e r c i r c u i t )8 // see a l s o end o f t h i s f i l e9 ex te rn vo id init_pwm( vo id ) ;

11 // Set speed s o f motors ( −128 . . .+127) . Nega t i v e v a l u e s12 // mean backward r o t a t i o n .13 ex te rn vo id motors ( s i gned char l e f t , s i gned char r i g h t ) ;

17 // In the d e f a u l t c o n f i g u r a t i o n wi th18 //#d e f i n e l e f t_d i r_p i n RB019 //#d e f i n e l e f t _ d i r _ t r i s TRISB020 //#d e f i n e r i g h t_d i r_p i n RB121 //#d e f i n e r i g h t _d i r _ t r i s TRISB122 // i n pwm. c , make the f o l l o w i n g c onn e c t i o n s :23 //24 // p in17 CCP1 <−−−> f i r s t motor c o n t r o l p i n ( marked enb l )25 // p in33 RB0 <−−−> second motor c o n t r o l p i n ( marked d )26 // p in16 CCP2 <−−−> th i r d motor c o n t r o l p i n27 // p in34 RB1 <−−−> fou rh motor c o n t r o l p i n28 //29 // l e f t motor <===> motor t e rm i n a l marked 1 ( two p i n s )30 // r i g h t motor <===> motor t e rm i n a l marked 2 ( two p i n s )31 //32 // then r e v e r s e the p o l a r i t y o f the motors by t u r n i n g33 // the c onn e c t o r s at the motor t e rm i n a l s i f n e c e s s a r y34 // u n t i l the motors run i n the d i r e c t i o n r e qu e s t e d by the35 // your program

A.5. ir_comm.h

1 // ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ir_comm . h

59

A. Source code

3 // De f i n e 2D a r r a y f o r s t o r i n g r e c e i v e d s i g n a l v a l u e s4 // One i n t pe r s e n s o r and t a r g e t5 // F i r s t i n d e x i s s e n s o r number ( 0 . . . 4 )6 // Second i nd e x i s t a r g e t number (0−puck , 1− of f_goa l , 2− def_goal ,7 // 3− robot , 4− umodulated )8 // i n bank 0 , 5 s e n s o r s , 5 1 6 b i t r e a d i n g s each9 ex te rn bank1 i n t target_map [ 5 ] [ 5 ] ; //don ’ t read data from he r e ; use

i r_va l u e ( ) macro

11 // used by i r_va l u e macro below12 ex te rn uns igned i n t temp_ir_val ;

14 // Macro to read an IR va l u e w i thou t r i s k i n g i n t e r f e r e n c e from the15 // i n t e r r u p t r o u t i n e . Always use t h i s to read IR v a l u e s16 // F i r s t i n d e x i s s e n s o r number ( 0 . . . 4 )17 // Second i nd e x i s t a r g e t number (0−puck , 1− of f_goa l , 2− def_goal ,18 // 3−opponent , 4− umodulated ) ; use d e f i n e s be low19 // Use l i k e i f ( i r_va l u e (0 ,PUCK)>THRESHOLD) do_something ( ) ;20 // # d e f i n e i r_va l u e ( sens , t a r g ) ( d i ( ) , temp_ir_val=target_map [ s en s ] [

t a r g ] , e i ( ) , temp_ir_val )21 // Commented away beacause the r i s k o f i n t e r r u p t r o u t i n e w r i t i n g to

the i n t v a l u e22 // be ing read j u s t between r e ad i n g the two by t e s i s v e r y sma l l and

the r e s u l t s not g rave .23 // Us ing i t w i th s e v e r a l t imes i n one e x p r e s s i o n ( e . g . compar ing to

s e n s o r s ) a l s o g i v e s24 // unexpected r e s u l t s . Th e r e f o r e we25 #de f i n e i r_va l u e ( sens , t a r g ) target_map [ s en s ] [ t a r g ]26 // i n s t e ad , j u s t f o r c o m p a t i b i l i t y w i th o ld code27 // To be a b s o l u t e l y s a f e a g a i n s t i n t e f e r e n c e from i n t e r r u p t s , use

d i ( ) and e i ( ) around28 // c r i t i c a l code segments ( they had b e t t e r be s ho r t though ) or29 // use wa i t_ i r ( ) i n a way tha t e n s u r e s tha t the i n t e r r u p t r o u t i n e

w i l l not mod i fy30 // the i r v a l u e s be i ng read when they a r e be i ng read .

32 // t a r g e t d e f s33 #de f i n e PUCK 034 #de f i n e OFF_GOAL 1

60

A. Source code

35 #de f i n e DEF_GOAL 236 #d e f i n e OPPONENT 337 #de f i n e UNMODULATED 4

39 ex te rn bank2 char i r_ r equ e s t e d ; // s e t one or more o f b i t s 0 . . 4 tor e qu e s t data f o r

40 // tho se s e n s o r s ; LOOK ATi r_wa i t_fo r ( ) FUNCTION

41 // BEFORE MODIFYING42 ex te rn bank2 char i r_ r e c e i v e d ; // each b i t i n d i c a t e s i f the

c o r r e s p ond i n g s e n s o r has43 // been r e c e i v e d ; used and c l e a r e d

by i r_wa i t_fo r ( )44 ex te rn bank2 char s p i_ f l a g s ; // some u s e f u l f l a g s hand led by

i n t e r r r u p t r o u t i n e45 // ( s ee be low )

47 // b i t d e f s f o r s p i f l a g s48 #de f i n e GOT_1ST 0 // used by i n t e r r u p t r o u t i n e to d i s t i n g u i s h

two c o n s e c u t i v e s t a r t b y t e s49 #de f i n e BAD_DATA 1 // s e t on f a i l u r e to r e c e i v e one ten b i t

s e t o f data f o r one s e n s o r50 // or r x ove r f l ow , c l e a r e d on r e c e p t i o n o f

s e n s o r 051 #de f i n e OVERFLOW 2 // s e t a f t e r r x ove r f l ow , c l e a r e d on

r e c e p t i o n o f s e n s o r 052 #de f i n e RECEIVING 3 // 1 when r e c e i v i n g s e t o f 1 2 b y t e s ove r

SPI e l s e 0

54 // t h e s e v a r i a b l e s c o n t r o l the s e r v o s connected to the co−p r o c e s s o r55 // u s ab l e range i s −5500 to +12500 ( though much l e s s shou ld be

enough on most s e r v o s )56 // check to s ee tha t your ext reme v a l u e s does not damage the s e r v o (

i t shou ld not57 // keep buzz i ng at i t s end p o s i t i o n i f un loaded )58 // s e r v o 0 i s to the l e f t on the motherbord ( second th r e e−p in header

from the d ip sw i t c h )59 ex te rn i n t s e r v opo s [ 4 ] ; //don ’ t w r i t e to t h i s one ( r e ad i n g i s OK)

61 // use t h i s macro to s e t s e r v o p o s i t i o n s

61

A. Source code

62 #de f i n e s e t_se r vo (num, pos ) d i ( ) ; s e r v opo s [ num]=pos ; e i ( ) ;63 // ( read s e r v o p o s i t i o n s d i r e c t l y from s e r v opo s a r r a y )

66 // i n i t SSP module i n SPI mode67 // I /O p i n s used RC3 , RC4 , RC5 ( TRISC b i t s 3 , 4 , 5 a r e hand led he r e

)68 ex te rn vo id i r _ i n i t ( ) ;

70 // c a l l t h i s f u n c t i o n i n the main l oop to keep i t s y n ch r on i s e d71 // to the a r r i v a l o f new IR measurements from the co−p r o c e s s o r72 // ( i f d e s i r e d ) .73 //74 // Waits f o r a new r e c e p t i o n o f a l l s e n s o r s to complete75 // The argument i s the s e n s o r number to wa i t f o r , which becomes76 // the l a s t one b e f o r the wa i t i n g i s ove r and i r_wa i t r e t u r n s .77 // I f a l l s e n s o r s a r e read ( s e t by i r_ r equ e s t e d d e c l a r e d above )78 // use e . g . 4 to read s e n s o r s 0−4 and then r e t u r n .79 // BE SURE NOT TO WAIT FOR A SENSOR THAT HAS NOT BEEN REQUESTED80 // THROUGH i r_ r equ e s t e d !81 ex te rn vo id i r_wa i t_fo r ( char s e n s o r ) ;

83 // as a d e f a u l t t h i s macro can be used , which wa i t s f o r s e n s o r 484 //THIS CAN ’T BE USED IF SENSOR 4 IS NOT REQUESTED THROUGH

i r_ r equ e s t e d !85 #de f i n e i r_wa i t ( ) i r_wa i t_fo r (4 )

A.6. debug.h

1 // ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ debug . h

3 // to use t h i s s imp l e debugg ing t o o l you have to know the add r e s s o fe v e r y

4 // v a r i a b l e to be mon i to red . i t can show the c on t en t s o f e v e r y bytei n bank

5 // 0 , 1 , 2 , and 3 and i n eeprom . i n t e g e r s e t c can on l y be v iewed ass e p a r a t e

62

A. Source code

6 // by t e s .

8 //a c a l l to t h i s f u n c t i o n i s b l o c k i n g and s t op s a l l o t h e r programex e cu t i o n

9 // i n c l u d i n g the i n t e r r u p t r o u t i n e10 // i t i s used to v iew the c on t en t s o f the f i l e r e g i s t e r s and the

EEPROM11 // i t u s e s t h r e e add r e s s r ange s ( use menu button to cheange between

them ) :12 //B01 − a d d r e s s e s 0 to 255 i n banks 0 and 113 //B23 − a d d r e s s e s 2 56 to 511 i n banks 2 and 3 ( the d i s p l a y w i l l

s t i l l show 0 to 255)14 //EEP − a d d r e s s e s 0 to 255 i n the EEPROM of the d e v i c e15 //Program FLASH can not be read16 //The add r e s s shown i s changed wi th the up and down but ton s17 //The format o f the data can be changed between18 // hexadec ima l , dec ima l , b i n a r y and a s c i i c h a r a c t e r modes19 // by p r e s s i n g the menu button20 ex te rn vo id debug ( ) ;

A.7. sprint.h

1 // ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ s p r i n t . h

3 // p r i n t an i n t to buf i n dec ima l fo rmat4 // buf has to be i n bank 0 or 1 or s t r a ng e5 // ’ f i x u p o v e r f l ow ’ e r r o r s w i l l o c cu r at6 // l i n k t ime7 ex te rn vo id s p r i n t 1 6 ( char ∗ buf , i n t num) ;

10 // p r i n t a char to buf i n hex fo rmat11 // buf has to be i n bank 0 or 1 or s t r a ng e12 // ’ f i x u p o v e r f l ow ’ e r r o r s w i l l o c cu r at13 // l i n k t ime14 ex te rn vo id s p r i n t 8X ( char ∗ buf , char num) ;

63

A. Source code

A.8. adc.h

1 // ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ adc . h

4 // sample an A/D channe l and r e t u r n the e i g h t most5 // s i g n i f i c a n t b i t s ( the two l e a s t s i g n i f i c a n d can be6 // f e t c h ed from ADRESL as i t s b i t 6 and 7)7 //8 // Thi s f u n c t i o n i n c l u d e s a d e l a y f o r the r e q u i r e d9 // a c q u i s i t i o n t ime ( enough f o r s ou r c e impedances up

10 // to 10kOhms) and then wa i t s f o r the c on v e r i o n to11 // complete . A l l i n a l l a c a l l w i l l t ake about 40 us .12 //13 // ( wa i t i n g f o r the a c q u i s i t i o n t ime i s r e a l l y a waste14 // o f t ime i f the same channe l i s sampled aga in and15 // not immed ia t e l y a f t e r the p r e v i o u s c o n v e r s i o n )16 //17 //Do not f o r g e t to c o n f i g u r e which p i n s you w i l l u se18 // f o r A/D i n ADCON1 and TRISA ( and maybe TRISE too )19 // once b e f o r e f i r s t u s i n g t h i s f u n c t i o n20 // Example ( i f you want on l y one A/D channe l ) :21 //ADCON1=0b00001110 ; TRISA0=1;22 // Example ( i f you want 5 A/D channe l s ) :23 //ADCON1=0b00000010 ; TRISA0=TRISA1=TRISA2=TRISA3=TRISA5=1;24 // See f i g 11−2 i n data s h e e t25 //26 //The p in each channe l u s e s i s marked on the f r o n t page27 // o f the data s h e e t as ANx , wher x i s the channe l number28 ex te rn char sample_ad_channel ( char channe l ) ;

A.9. lcd.h

1 // ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ l c d . h

3 //macro to power the l c d on and o f f4 #de f i n e set_lcd_power ( on ) (RD4=on )

64

A. Source code

6 // get the l c d go ing a f t e r power−up7 // the l c d u se s a l l o f po r t D8 ex te rn vo id i n i t _ l c d ( ) ;

10 // f u n c t i o n s to w r i t e to the LCD:

12 // p r i n t to d i s p l a y mem l i n e 1 at p o s i t i o n pos13 // s t r i s a n u l l t e rm ina t ed a r r a y o f c h a r s14 ex te rn vo id l c d_p r i n t 1 ( const char ∗ s t r , char pos ) ;

16 // p r i n t to d i s p l a y mem l i n e 2 at p o s i t i o n pos17 // s t r i s a n u l l t e rm ina t ed a r r a y o f c h a r s18 ex te rn vo id l c d_p r i n t 2 ( const char ∗ s t r , char pos ) ;

20 // p r i n t to d i s p l a y mem at the c u r r e n t c u r s o r p o s i t i o n on l i n e 1 or2

21 // s t r i s a n u l l t e rm ina t ed a r r a y o f c h a r s22 ex te rn vo id l c d_pr i n t_at_cur so r ( const char ∗ s t r ) ;

24 // c l e a r the d i s p l a y25 ex te rn vo id c l e a r_d i s p ( ) ;

A.10. defines.h

1 // ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ d e f i n e s . h

3 // Macros f o r b i t man i pu l a t i o n and t e s t i n g4 #de f i n e b i t s e t ( var , b i t n o ) ( ( va r ) |=1 << ( b i t n o ) )5 #de f i n e b i t c l r ( var , b i t n o ) ( ( va r )&=~(1<<(b i t n o ) ) )6 #de f i n e b i t t s t ( var , b i t n o ) ( ( ( va r )&(1<<(b i t n o ) ) ) !=0)

8 // macros f o r man i pu l a t i n g h i gh and low byte o f an i n t v a r i a b l e9 #de f i n e l o_byte ( i n t_va r ) ( ∗ ( ( char ∗ ) (&( in t_va r ) ) ) )

10 #de f i n e hi_byte ( i n t_va r ) ( ∗ ( ( ( char ∗ ) (&( in t_va r ) ) ) +1 ) ) /∗workson l y f o r bank0 va r s ∗/

11 #de f i n e hi_byte2 ( i n t_va r ) ( ∗ ( ( ( bank2 char ∗ ) (&( in t_va r ) ) ) +1 ) )

65

A. Source code

/∗works on l y f o r bank2 va r s ∗/

66

B. Circuit Diagrams

67

B. Circuit Diagrams

1 2 3 4 5 6 7 8

A

B

C

D

E

F

G

H

1 MCLR2 RA0/AN03 RA1/AN14 RA2/AN2

9 RE1/AN610 RE2/AN7

8 RE0/AN5

7 RA5/AN4

11 Vdd12 Vss13 OSC114 OSC215 RC016 RC1/CCP217 RC2/CCP118 RC3/SCK

5 RA3/AN3/Vref6 RA4/T0CKi

19 RD020 RD1 21RD2

22RD3

23RC4/SDI

24RC5/SDO

25RC6

26RC7

27RD4

28RD5

29RD6

30RD7

31Vss

32Vdd

33RB0/INT

34RB1

35RB2

36RB3

37RB4

38RB5

39RB6

40RB7

MC1

PIC16F877

Vcc

C10.1uF

Vcc

Y120M

C230p

C3

30p

1 R1

2 R2

3 R3

4 R4

5 R5

6 R6

7 R7

8 R8

9CM

RR1

8x470

1R1

2R2

3R3

4R4

5R5

6R6

7R7

8R8

9 CM

RR2

8x4.7k

Vcc

12345678

J1

GND

1 R1

2 R2

3 R3

4 R4

5 R5

6 R6

7 R7

8 R8

9CM

RR3

8x470

12345678

J2

Vcc

C40.1uF

RR4

8x470 Y220M

C530p

C630p

123

J3

IR0

R1100k

Vcc

123

J4

IR1

R2100k

Vcc

123

J5

IR2

R3100k

Vcc

123

J6

IR3

R4100k

Vcc

123

J7

IR4

R5100k

Vcc

12345678

J8

MOT_CTRL

U1A

U1B

U1C

U1D

74HCT04

1R1 2R2 3R3 4R4 5R5 6R6 7R7 8R8

9 CM

RR5

8x4.7k

1 ENBL1

2 INP1 3OUT1

4 GND5 GND

6OUT27 INP2

8Vs9 ENBL2

10 INP3 11OUT3

12GND

13GND

14OUT415 INP4

16Vss

U2 L293DN

1 ENBL1

2 INP1 3OUT1

4 GND5 GND

6OUT27 INP2

8Vs9 ENBL2

10 INP3 11OUT3

12GND

13GND

14OUT415 INP4

16Vss

U3 L293NE

Vcc

C70.1uF

C80.1uF

C9

0.1uF

Vcc

C10

22uFC11 0.47uF

Vcc

123

J9

BATT

Vs

Vcc

1 2 3

J10 CTRL

1234

J11

MOTOR1

1234

J12

MOTOR2

12345678910

J13

12345678910

J14

123456789

10

J15

J16

2G

ND

1 IN 3OUT

U4 LM2940

R6

81

R7

4.7k

13579111315

2468

10121416

J17

8LEDS

13579111315

2468

10121416

J18

8JPS

13579111315

2468

10121416

J19

8LEDS

13579111315

2468

10121416

J20

8LEDS

13579111315

2468

10121416

J21

8JPS

Vcc

S2

1 R12 R23 R34 R45 R56 R67 R78 R8

9CM

RR6

8x4.7k

gnd

RA4 is open drain

(74HCT04)

12345678J23 V6, Vcc Vcc

R9

8.2, 6W

750 mA max

C13 0.33uF

V8Vs

2G

ND

1 IN 3OUT

U5 7808CVR10

3.9, 6W

1 A max

V8

12345678J24 GND

V8

Vs

S3

Locomotion PWR

S4

5V

8V

Vcc

C00.1uF

12345J22 SERVO

S1

RESET

C120.1uF

++-

MC2

PIC16F876

14.4V in

RAS Motherboard Schematic

Pre-programmed PIC

Main PIC

Motor Drivers

Voltage regulators

Figure B.1.: The layout of the robot’s motherboard [3]

68

Bibliography

[1] Ronald C. Arkin. Behavior-Based Robotics, chapter 1, 3 and 4. MIT Press,1998.

[2] Mattias Bratt. Lecture L2 - the microcontroller of the course. Lecture Slides,2D1426 Robotics and Autonomous Systems, September 2003.

[3] Mattias Bratt. Lecture L3 - the project. Lecture Slides,2D1426 Robotics and Autonomous Systems, September 2003.http://www.nada.kth.se/kurser/kth/2D1426/L3_2003b.pdf.

[4] Anders Conradi, Anders Dahl, Per-Johnny Käck, and Joel Sjöstrand. FejkO-paque. Project Report, 2D1426 Robotics and Autonomous Systems, May 2003.http://www.nada.kth.se/kurser/kth/2D1426/reports2003/FejkOpaque.pdf.

[5] Alexander Enstedt, Magnus Jansson, Ville Lumikero, and Christian Schüldt.Matris. Project Report, 2D1426 Robotics and Autonomous Systems, May 2003.http://www.nada.kth.se/kurser/kth/2D1426/reports2003/Matris.pdf.

[6] Joseph L. Jones and Anita M. Flynn. Mobile Robots - Inspiration to Implement-

ation, chapter 9 - Robot Programming. A. K. Peters Ltd., 1993.

69

purple haze - kth€¦ · this report describes the design of a small autonomous hockey-playing...

Documents