COMPETING THREADS VS - People | Computer...

COMPETING THREADS VS. COLLABORATING THREADS IN EMBEDDED SYSTEMS

by

LETCHUMANAN MUTHAIAH

B.E., University of Madras, India, 2001

----------------------------------

A REPORT

submitted in partial fulfillment of the

requirements for the degree

MASTER OF SCIENCE

Department of Computing and Information SciencesCollege of Engineering

KANSAS STATE UNIVERSITYManhattan, Kansas

2003

Approved by:

Major ProfessorDr. Masaaki Mizuno

1

ABSTRACT

leJOS (version 2.1.0) is a programming language for the Lego Mindstorm computer

brick called RCX (Robotic Command eXplorer). The leJOS API is a trimmed down

version of the Java API along with classes to control motors, sensors and other

components of the RCX brick. While studying the source code for the leJOS class

library, we found that implementation of some of the classes can be improved to

make it more suitable for a real time system. Also, the implementation had 3 threads

competing with each other to obtain the CPU of the RCX using busy waiting with

certain critical sections not properly protected by locks. We rewrote two of the major

classes of leJOS and developed two versions of the implementation; a time-triggered

and an event triggered multithreaded implementation in which the threads

collaborate, rather than compete, with each other for the common goal. These

implementations maintain the same interface as the API of the original

implementation, therefore the application programs using the leJOS API work

reliably by replacing the original classes with the newly implemented ones.

Moreover the studying and modifying of leJOS code has made us understand the

significance and indispensability of using collaborative threads in the development of

efficient concurrent programs.

2

TABLE OF CONTENTS

1 INTRODUCTION.....................................................................................................1

1.1 THE ROBOTICS INVENTION SYSTEM....................................................................11.1.1 RCX Brick........................................................................................................1

1.2 LEJOS...................................................................................................................21.3 BEHAVIOR CONTROL............................................................................................4

1.3.1 leJOS Behavior API.........................................................................................51.4 NAVIGATION.........................................................................................................6

1.4.1 The Navigator API...........................................................................................7

2 TIPPY SENIOR.......................................................................................................10

2.1 DESCRIPTION......................................................................................................102.2 BEHAVIORS........................................................................................................102.3 TEST RUN...........................................................................................................112.4 SOURCES OF ERROR...........................................................................................17

3 COLLABORATING CONCURRENT PROGRAMS.........................................20

3.1 CONCURRENT PROGRAMS..................................................................................203.2 COLLABORATING AND COMPETING THREADS...................................................213.3 EVENT TRIGGERED IMPLEMENTATION...............................................................223.4 TIME TRIGGERED IMPLEMENTATION.................................................................27

4 PERFORMANCE AND ACCURACY..................................................................30

5 CONCLUSION........................................................................................................31

6 REFERENCES........................................................................................................32

APPENDIX A: LEJOS ORIGINAL IMPLEMENTATION, VERSION 2.1.0..........33

APPENDIX B: EVENT TRIGGERED IMPLEMENTATION..................................54

Appendix C: Time Triggered Implementation..................................................................66

i

LIST OF FIGURES

FIGURE 1-1: THE LEJOS MEMORY MAP [1]......................................................................................................3

FIGURE 1-2: HEIRARCHY OF BEHAVIOR [1].....................................................................................................4

FIGURE 1-3: DEAD RECKONING [1]..................................................................................................................6

FIGURE 2-1: BEHAVIOR MOVE ……………………………………………………………………...……....

11

FIGURE 2-2: BEHAVIOR CENTERBUMP….………………………………………………………...

……........12

FIGURE 2-3: BEHAVIOR GOHOME…………………………………………………………...……………..

13

FIGURE 2-4: ROTATIONNAVIGATOR, ORIGINAL IMPLEMENTATION................................................................16

FIGURE 2-5: TIMER, ORIGINAL

IMPLEMENTATION..........................................................................................19

FIGURE 3-1: EVENT TRIGGERS........................................................................................................................23

FIGURE 3-2: SOLUTION TO LOST NOTIFICATION.............................................................................................24

FIGURE 3-3: ARBITRATOR, ET

IMPLEMENTATION...........................................................................................25

FIGURE 3-4: ROTATTIONNAVIGATOR, ET IMPLEMENTATION.........................................................................26

FIGURE 3-5: ARBITRATOR, TT

IMPLEMENTATION...........................................................................................28

FIGURE 3-6: ROTATIONNAVIGATOR, TT IMPLEMENTATION…………………………………………….......

29

ii

ACKNOWLEDGEMENTS

I am very grateful to have had the opportunity to work with my major professor,

Dr.Masaaki Mizuno. His energy, enthusiasm and desire to thoroughly understand the

subject at hand has greatly inspired me. I would also like to thank him for his constant

guidance through out various projects and coursework.

I would like to thank Dr. Gurdip Singh and Dr. Mitchell Neilsen for serving on my

committee.

I would like to thank Mr. Walamitien Hervé Oyenan and Mr. Sreenivasa Babu

Kondannagari who have also worked with me on the various other facets of the project.

I would also like to extend my gratitude to Ms. Delores Winfough and other staff

members of the Department of Computing and Information Sciences for guiding me

through the academic and graduation procedures.

iii

I would like to thank my friends and family for their encouragement and support during

my MS program.

To

my Parents

iv

1 Introduction

1.1 The Robotics Invention System

The MINDSTORMS is a product from LEGO that allows you to design and program

robots. The concept for this was originally developed by the partnership of LEGO and

Massachusetts Institute of Technology. At the core of this product range is the Robotics

Invention System (RIS). The RIS (as of version 2.0) kit consists of 717 individual parts,

including motors, touch sensors, light sensors, infrared transmitter, bricks, pulleys and

gears.

1.1.1 RCX Brick

At the heart of the RIS is the Robotic Command eXplorer (RCX) brick, which acts as the

brain for the MINDSTORMS products. It houses a Hitachi H8/3292 series micro-

controller, which has a 16Kb ROM, 0.5 Kb RAM (on board) and a clock speed of

16MHz. The firmware on the ROM provides for communication between motors and

sensors, displaying data on the LCD and downloading programs from the computer. The

RCX also contains 32Kb external RAM where the user programs will be stored.

There are 3 inputs ports numbered 1, 2 and 3 to which the sensors can be connected.

These ports have the LEGO studs with metallic connectors for half their circumference

1

that connect to the underside of the wires from the sensor. There are also 3 output ports

A, B and C to which the actuators can be connected.

The RCX also has 4 buttons, On-Off, Run, View and Prgm. The On-Off button turns the

power on and off for the RCX. There can be a maximum of 5 programs stored in the

RCX at one time and the Run button starts executing the selected program. The Prgm

button is used to make the selection among the different programs. The View button

alternates between the following views: current time, input sensors and output ports. In

the case of sensors, based on how they have been programmed, a Boolean value (0 or 1)

or Percentage (0-100) or a raw value is displayed.

The RCX has an infrared port that allows two-way transmission of data, which is used for

downloading programs from the computer and for two RCX’s to communicate with each

other. There is a battery compartment on the underside that holds the 6 AA batteries that

power the RCX.

1.2 leJOS

leJOS is a Java platform for the RCX brick [1]. The leJOS API includes a pruned version

of the Java API along with classes to control the motors, sensors and other components of

the RCX brick. The packages that are included in leJOS are java.lang, java.util, java.io

with reduced number of interfaces, classes and exceptions than their counterparts in the

Java API. In most cases identical method names and return types are used to maintain

consistency. It supports threads, arrays, Java event model and exception handling. It also

allows recursion, but limits it to a depth of 10 levels. Another point that should be noted

2

is that automatic garbage collection is not done. The RCX specific classes control the

motors, sensors, buttons, LCD, battery power, speaker, IR communications, timers and

system time. leJOS also includes several classes and interfaces for behavior control and

navigation to allow for robotics programming.

Programs written for the RCX using leJOS have a ‘.java’ extension. They can be

compiled by saying ‘lejosc <program name.java>’, which actually uses the Sun Java

compiler javac. The class files are converted into a single binary file and uploaded into

the RCX by saying ‘lejos <program name>’, which will then be interpreted by the Java

Virtual Machine (JVM) that resides in the RAM of the RCX. As mentioned earlier the

RCX has a 32Kb external RAM; of which JVM uses up 16 Kb, the ROM routines reserve

for themselves a space of 4 Kb leaving behind 12 Kb of memory for the user programs.

Figure 1-1: The leJOS memory map [1]

16 Kb ROM

16Kb leJOS JVM

12 KbUser Programs

4 Kb Off Limits

3

1.3 Behavior Control

Behavior Control is a reactive robot control architecture that was first proposed by

Rodney Brooks in one of MIT’s AI report [3]. Brooks developed this concept based on

his observation of insects in the real world. A complex behavior of the insect was nothing

but a series of alternations of its simple behaviors with one another. Brooks adapted this

to robot control architecture to come up with what was originally known as subsumption

architecture. A behavior is a task that processes some sensory information, either

external or internal, and issues an action. To coordinate the final action, each behavior

can disable some of the other behaviors, which conflict with its own action. This is based

on the priority assigned to the behaviors and this capability to override other commands

is called subsumption [4]. Ordering of the behaviors can be shown, as below, using the

standard representation given by Brooks.

Figure 1-2: Hierarchy of Behavior [1]

Touch Sensor

Collision Behavior

Drive to Start Point

Back & spray extinguisher

Drive toward heat

Battery Level

TemperatureSensor

Fire extinguisher

Motors

S

S

S

S Point of Suppression

4

1.3.1 leJOS Behavior API

We will now see how leJOS implements this behavior control. It provides a single

interface, josx.robotics.Behavior, to define behaviors. Once the required set of behaviors

for the robot has been designed, it is given to an arbitrator, defined by the class

josx.robotics.Arbitrator. The arbitrator then decides which behavior should be activated.

The API of the Behavior interface includes the following method declarations

boolean takeControl()

void action()

void suppress()

The takeControl method returns a Boolean value indicating if that particular behavior

should be active or not. A Boolean value of true means this behavior needs to be active

and vice versa. The action method contains the code corresponding to the action that has

to be executed when the behavior is active. Finally, the suppress method contains code

that is used to stop the action and update the necessary data.

The API for the Arbitrator has the following method declarations

public Arbitrator(Behavior[] behaviors)

public void start()

After instantiating the Arbitrator object and providing it with an array of Behaviors the

start() method is called. The Arbitrator then keeps going through the Behaviors checking

if any of them want to take control. Once a ready Behavior is found, it executes the

5

action() method of the behavior. Meanwhile it continues to go through the array and

whenever a higher priority Behavior is ready to take control, the suppress() method of the

currently executing Behavior is called to stop it and then the action() of the new Behavior

is executed.

1.4 Navigation

Before the 16th century sailors navigated based on a method called deduced reckoning or

dead reckoning. In this, the navigator finds his position by measuring the course and the

distance he has traveled from a known point. For some time now this method has been

adopted for robot navigation. Knowing the starting coordinates, the angle and distance

traveled by the robot, we can find out the destination coordinates.

Figure 1-3: Dead Reckoning [1]

x = cos(angle) * distance

y = sin(angle) * distance

6

Dead reckoning is less expensive when compared to other navigational systems and the

computation involved is very simple.

1.4.1 The Navigator API

The leJOS Navigator API is based on dead reckoning navigation and caters to robots with

differential steering. It provides methods for moving a particular distance and rotating a

certain angle. The robot’s current x, y coordinates and angle are maintained by the

Navigator object and are updated after every movement. The interface for the Navigator

is as follows

public float getX() : returns the current X coordinate of the robot

public float getY() : returns the current Y coordinate of the robot

public float getAngle(): returns the current angle of the robot

public void forward(): moves the robot forward until stop is called

public void backward(): moves the robot backward until stop is called

public void stop(): stops the robot and updates the X and Y coordinates

public void travel(int distance): moves the robot for the given distance; a positive

value indicates a forward movement and negative value indicates a backward

movement

public void rotate(float angle): rotates the robot for the given angle; a positive angle

indicates a counterclockwise rotation and negative angle indicates a clockwise

rotation

public void gotoAngle(float angle): rotates the robot through the shortest path to make

it orient in the given angle

7

public void gotoPoint(float x, float y): rotates the robot to face the destination and

moves it through the required distance to reach the target point

leJOS provides two classes that use this interface. The difference between them is the

way in which the angle and distance, needed for dead reckoning navigation, is measured.

The first one called the TimingNavigator measures the movement in terms of the number

of seconds it has traveled. When instantiating this navigator it should be provided with

time taken by the robot to move 100 units and to complete one full rotation. Once this is

done, to move a particular distance it just calculates the number of seconds it should let

motors run before coming to a stop.

The second and the more accurate one called the RotationNavigator measure the

movement in terms of the number of rotations of the robot’s wheels. A rotation sensor is

connected to the axle of the wheels of the robot for this. For every 22.5 degrees turned by

the wheel the RCX increments the rotation count by 1. That is, for every full rotation of

the wheel, the count goes up by 16. Turning the wheels in the reverse direction

decrements the count in the same manner as described. The RCX can count up to a speed

of 600 rotations per minute. The instance of this navigator is provided with the wheel

diameter and drive length. When the robot’s wheel makes one turn it travels a distance

equal to the circumference of the wheel. Knowing the wheel diameter and hence the

circumference, distance to be traveled can be broken down into number of rotations of

the wheel. Sixteen times this is the count up to which the RCX has to measure to before it

can stop the motors. The drive length is nothing but the distance between the 2 wheels.

When the robot makes a full 3600 turn to the left, the left wheel rotates backward and the

8

right wheel rotates forward for a distance equal to the circumference of a circle whose

diameter is equal to the drive length. When the robot has to turn a lesser angle the wheels

will rotate for a fraction of this circumference.

9

2 Tippy Senior

2.1 Description

To test the leJOS code, we needed to run it on a robot. For which we chose “Tippy

Senior” a simple and easy to build navigator robot that was given in [1]. The RCX brick

was mounted on top of a chassis, built with LEGO plates and beams. Two motors and

two rotation sensors, one for each wheel, were attached at the bottom. The gear train from

the wheel axle to rotations sensors axle was such that the rotation sensor axle rotated

three times for every one rotation of the wheel. A touch sensor was placed at the front

and a bumper was also added. Whenever the robot hits an obstacle the bumper would be

forced inwards making contact with the touch sensor.

2.2 Behaviors

Tippy Senior has 3 behaviors Move, CenterBump and GoHome. The behavior Move is

Tippy Senior’s normal behavior, it randomly calculates a new x,y coordinate and moves

to this new destination. This process is repeated again and again. While this is being

done, Tippy might hit against an obstacle, that is when CenterBump, a slightly higher

priority behavior kicks in. This causes it to backtrack for 20 units, from where it will find

a new x, y coordinate to move to. The third behavior GoHome has the highest priority.

After 30 seconds have elapsed this behavior executes causing the robot to travel back to

home or coordinates (0,0). The implementation for all these behaviors were taken out of

[7], with just a minor change made in the case of bump. The book implementation

superfluously had two behaviors called LeftBump and RightBump, which were

consolidated to a single CenterBump class.

10

2.3 Test Run

When we did the test run on Tippy Senior, we noticed that it completed its run under

tolerant conditions involving smaller number of obstacles, albeit with problems like

missing home(0,0) completely and/or missing the 30 second timeout to return home.

Under more strenuous conditions the earlier mentioned problems occurred more often

and with a greater magnitude of error. When Tippy received 2 consecutive bumps in a

very short period of time, a deadlock was encountered, with Tippy just freezing in its

tracks.

import josx.robotics.*;public class Move implements Behavior { private boolean active; // Indicates behavior is active private Navigator nav;

public Move(Navigator nav) { this.nav = nav; active = false; }

public boolean takeControl() { return true; }

public void suppress() { active = false; nav.stop(); }

public void action() { active = true; while(active) { float x = (int)(Math.random() * 150); float y = (int)(Math.random() * 150); nav.gotoPoint(x, y); } }}

Figure 2-4: Behavior Move

11

/*************** CenterBump.java ******************/

import josx.robotics.*;import josx.platform.rcx.*;import josx.util.*;

public class CenterBump implements Behavior, SensorListener {private Navigator nav;private boolean hasCollided;

public CenterBump(Navigator nav) {this.nav = nav;hasCollided = false;Sensor.S2.addSensorListener (this);

}

public boolean takeControl() {if (hasCollided) {

hasCollided = false; // reset valuereturn true;

}else return false;

}

public void suppress() { nav.stop();}

public void stateChanged(Sensor bumper, int oldValue, int newValue) {

if (bumper.readBooleanValue() == true){

hasCollided = true;}

}

public void action() { // back up Sound.beep(); nav.travel(-20);}

}

Figure 2-5: Behavior CenterBump

12

import josx.robotics.*;import josx.platform.rcx.*;import josx.util.*;

public class GoHome implements TimerListener, Behavior { Navigator nav; boolean timeUp; public GoHome(Navigator nav) { this.nav = nav; timeUp = false; Timer t = new Timer(30000, this); t.start(); } public void timedOut() { timeUp = true; } public boolean takeControl() { return timeUp; } public void suppress() { nav.stop(); } public void action() { Sound.beepSequence(); nav.gotoPoint(0,0); Sound.twoBeeps(); try {Thread.sleep(5000);}catch(InterruptedException e) {} Sound.beep(); timeUp = false; // reset time up }}

public void rotate(float angle) { // keep track of angle this.angle = this.angle + angle; this.angle = (int)this.angle % 360; // Must be < 360 degrees // Is it possible to do the following with modulo (%) ??? while(this.angle < 0) this.angle += 360; // Must be > 0 // Calculate the number of intervals of rotation sensor to count int count = (int)(COUNTS_PER_DEGREE * angle); rotLeft.setPreviousValue(0); rotRight.setPreviousValue(0); if (angle > 0) { right.forward(); left.backward(); while(rotLeft.readValue() > (count*-1) || rotRight.readValue() < count) {}

} else if (angle < 0) { right.backward(); left.forward(); while(rotLeft.readValue() < (count*-1) || rotRight.readValue() > count) {} }

right.stop(); left.stop(); }public void travel(int dist) { // !! The command != STOP lines need to be tested!!! // !! Should stop and exit travel() when travel() interrupted by stop() int counts = (int)(dist * COUNTS_PER_CM); if(dist > 0) { forward(); while(command != STOP && (rotLeft.readValue() < counts || rotRight.readValue() < counts)) { Thread.yield(); } } else if(dist < 0) { backward(); while(command != STOP && rotLeft.readValue() > counts || rotRight.readValue() > counts) { Thread.yield(); } } stop(); }

Figure 2-6: Behavior GoHome

13

public void stop() { if(moving) { command = STOP; while(moving) { Thread.yield(); } left.stop(); right.stop(); // Recalculate x-y coordinates based on rotation sensors int rotAvg = (rotLeft.readValue() + rotRight.readValue()) / 2; float centimeters = rotAvg / COUNTS_PER_CM; // update x, y coordinates x = x + (float)(Math.cos(Math.toRadians(angle)) * centimeters); y = y + (float)(Math.sin(Math.toRadians(angle)) * centimeters); } } private class SteerThread extends Thread { public void run() { while(true) { while(command == FORWARD) { left.forward(); right.forward(); moving = true; if(rotLeft.readValue() > rotRight.readValue()) { left.flt(); while(rotLeft.readValue() > rotRight.readValue()) {} left.forward(); } if(rotRight.readValue() > rotLeft.readValue()) { right.flt(); while(rotRight.readValue() > rotLeft.readValue()) {} right.forward(); } Thread.yield(); }

15

Figure 2-7: RotationNavigator, original implementation

2.4 Sources of Error

while(command == BACKWARD) { left.backward(); right.backward(); moving = true; if(rotLeft.readValue() < rotRight.readValue()) { left.flt(); while(rotLeft.readValue() < rotRight.readValue()) {} left.backward(); } if(rotRight.readValue() < rotLeft.readValue()) { right.flt(); while(rotRight.readValue() < rotLeft.readValue()) {} right.backward(); } Thread.yield();

} moving = false; Thread.yield(); } }}

16

The methods of the RotationNavigator are accessed by the BehaviorAction thread (in the

Arbitrator) which executes the action method of each Behavior. Both BehaviorAction and

SteerThread access some of the same variables of the RotationNavigator such as moving

and command. These shared variables are not accessed in a synchronized manner.

The primary thread which calls the Arbitrator’s start method is executing in an infinite

loop checking to see if a higher priority behavior is ready to take control. The

BehaviorAction thread keeps checking the rotation sensor values until stop is called or the

robot has traveled the specified distance. As long as the robot is traveling the SteerThread

is continuously checking rotation sensor values of both the wheels to balance their

rotations. Even though these threads yield from time to time, conceptually they run in

busy-waiting, competing with each other.

The updating of x,y coordinates is done in the stop method of the RotationNavigator. This

method is called at the end of travel and also by the suppress method of the behaviors. As

this is not protected by synchronization, a race condition might result while updating x,y

throwing the whole coordinate system into disorder.

Another problem was noticed with the timer implementation. Timer thread once started

sleeps for the specified time and then informs the TimerListener. This is performed in an

infinite loop until the Timer’s stop method is called. Just after the listener is informed

and even before the stop method can be called the thread would go for one more iteration

and inform the listener again and only then check the condition to see if it should loop

17

again or not. The stop method does not interrupt this extra iteration of the Timer thread

which might lead to erroneous results.

package josx.util;public class Timer{ private TimerListener myListener; private Thread myThread ; private int delay ; private boolean running ; public Timer(int theDelay, TimerListener el) {

running = false;delay = theDelay;myListener = el;myThread = new Thread() { public void run() {

int d;boolean r;while(true) { synchronized(Timer.this) { d = delay; r = running; } if (r) { try { Thread.sleep (d); } catch (InterruptedException e) {} myListener.timedOut(); } else { yield(); }}

}};

} public synchronized int getDelay() {

return delay; } public synchronized void setDelay(int newDelay) {

delay = newDelay; } public synchronized void stop() {

running = false; } public synchronized void start() {

running = true;if (!myThread.isAlive()) myThread.start();

}

Figure 2-8: Timer, original implementation

18

3 Collaborating Concurrent Programs

19

3.1 Concurrent Programs

An execution of concurrent program is the interleaving of the atomic actions executed by

individual processes [8]. In the case of sequential program, the control moves from one

atomic action to another in a serial manner. In concurrent programs multiple processes

are present and hence multiple threads of control. These threads interleave to produce the

concurrent execution. Concurrent Programming has the following advantages:

Resource Utilization: When a thread must wait for some event, switching to a

different thread results in the better utilization of the CPU.

Speedup: Faster processing can be achieved for 2 reasons; efficient resource

utilization means a ready process need not be put on hold because a process

having the CPU is waiting for some other resource. The second reason is that the

different threads can be scheduled on different processors to achieve greater

parallelism.

Models real-life scenario: For problems that have concurrent activities, it is easier

to come up with a solution that has a concurrent approach; it is easier to write a

concurrent program than a sequential program in which switching from one

activity to another has to be done explicitly by the programmer.

Fair service: Time-sharing provides fair service. Suppose job say J1 has a very

long execution time and job J2 has very short execution time. A sequential

execution of J1 followed by J2 would make the response time of J2 unfairly long.

3.2 Collaborating and Competing Threads

20

Threads in a concurrent program are said to be collaborative threads if they co-operate

with each other in executing a task. Competing threads are those which either, are selfish

and do not give other threads a chance or those which just do yield or sleep but not

properly coordinate with others in achieving the common goal.

Competing threads, though a part of concurrent programs, never realize all the

advantages mentioned in the previous section. A proper balance of synchronizations and

yields should be present in a concurrent program without which the programmer has no

control over the way the threads are scheduled.

When a thread just yields without using any other coordination there is no way to say

which other thread will execute. The atomic actions of each of the concurrent processes

make indivisible program state transitions and different interleaving of the threads, at

various points in the execution, result in different traces of execution or different

histories. Some of these histories might be undesirable. To prevent such unacceptable

interleaving, to have a control over the threads and to add predictability to the execution

and outcome we need to use proper synchronization among threads.

There are two forms of synchronization. The first form called mutual exclusion is

achieved by combining fine-grained atomic actions into coarse grained or composite

actions. The second form known as condition synchronization is achieved by delaying a

process execution until the program state satisfies some predicate [8].

Again just as yielding without synchronizing produces undesirable results, so will

synchronizing without yielding. In this case the thread might wait only as long as it needs

21

some data, however when it has all the required data it might hog the CPU and never give

other threads a chance. This has very great significance in the case of real time and

embedded systems, which are mostly safety and mission critical applications. Here all the

threads will have to run simultaneously and cannot wait for a long time. This is different

from the way in which Operating System textbooks look at concurrency and hence the

ad-hoc synchronization techniques described by them do not apply.

3.3 Event Triggered Implementation

We will now propose an Event Triggered solution that corrects the errors of the original

implementation. The major source of error in the earlier code was the usage of competing

threads. This can be corrected by using proper synchronization between the threads and

each thread yielding for the other thread at relevant points. The synchronization code can

be easily developed using the structured high-level and efficient Pattern-based

methodology described in [9].

The threads that are involved are the primary thread that starts the Arbitrator, the

BehaviorAction thread that executes the action of each behavior and the SteerThread that

makes Tippy travel in a straight line.

The primary thread runs in an infinite loop checking to see if any new higher priority

behavior than the currently executing behavior is ready, if that is the case then it makes

this new behavior the current behavior and yields or otherwise it loops again without

yielding to check again. Actually the primary thread needs to run only in 3 cases

22

When an existing behavior completes its execution

When external event trigger occurs such as a bump

When an internal event trigger occurs such as a time out for go home.

Figure 3-9: Event Triggers

So the primary thread has to wait till any of these events notify it. But again there is

another problem here. Let us assume that the arbitrator is waiting and a lower priority

event notifies. The arbitrator wakes up and starts checking the next ready behavior,

meanwhile a higher priority event notify comes, but the arbitrator is not waiting any

more, therefore this notification is lost, which should not be the case. Using the

Asymmetric Barrier Pattern given in [9] solves our problem here.

Timer Listener

Touch Sensor Listener

Completion of Behavior

Rotation Sensor Listener

Behavior Thread

Steer Thread

Event Notification

23

Figure 3-10: Solution to lost notification

This increments a counter whenever a notification occurs thereby accounting for all the

notifications, the arbitrator then loops around that many times to see if it has missed any

higher priority trigger.

Event Listener

in++;notify();

Threadloop while　 (out >= in) wait( ); out++; process

24

public void wakeUpArbitrator() { synchronized(arbitratorLock) { in++; arbitratorLock.notify(); }

}public void start(Behavior [] behaviors) {

boolean waited = false;initArbitrator(behaviors);

while(true) {// Check through all behavior.takeControl() //starting at highest level behavior for(int i = totalBehaviors; i >= 0; --i) {

if(behavior[i].takeControl()) { if((i > currentBehavior)||(actionThread.current== NONE))

{ if(i>currentBehavior)

behavior[currentBehavior].suppress(); currentBehavior = i; actionThread.execute(i); break; } }

} synchronized(arbitratorLock) {

try { while(in<=out)

{arbitratorLock.wait(); waited = true;}out++;

}catch(InterruptedException e){}

}

if (! waited) Thread.yield();

//if it did not sleep, it may be better to give // other threads a chance to run, rather than// this thread keeps running.else

waited = false;

}}

Figure 33: Arbitrator, ET implementation

25

Coming to the SteerThread, it keeps on checking the sensor values to see if one of the

wheels has turned more than the other. If it has then it floats or stops that wheel for a

while letting the other wheel catch-up. Instead of continuously checking the sensor values

the thread can wait till there is a state change notification from the sensor. We will see a

more efficient solution for this in the next section.

public void stateChanged(Sensor rotation, int oldValue, int newValue) {

if (oldValue != newValue) {synchronized(steerLock) {

in++;steerLock.notify();

}} } // steering thread, wake up every 30 msec and check

steering based on the rotation sensor // readings

public void run() {

boolean waited = false; while(true) {

synchronized (this) { if(command == ROTATE) checkRotate(); else if(command == SLROTATE) checkSlowRotate(); else if(command == FORWARD) checkForward(); else if(command == BACKWARD) checkBackward(); } synchronized (steerLock) { try{ while (in <= out) {steerLock.wait(); waited =

true;} out++; }catch(InterruptedException e){} } if (! waited) Thread.yield();

// if it did not sleep, it may be better to give// other threads a chance to run, rather than// this thread keeps running.

else waited = false; } }

Figure 34: RotationNavigator, ET implementation

26

The other errors such as the concurrent access to variables like command and moving,

and updating of x,y coordinates can be corrected by providing a mutually exclusive

access. The accesses to these are always done within java’s synchronized block. With

proper synchronization the deadlock that occurred in the original implementation was

eliminated. Interrupting the Timer thread solves the problem of timer looping around

quickly one more time just before stop is called.

3.4 Time Triggered Implementation

Most embedded system applications in the real world are modeled as time-triggered (TT)

systems rather than event-triggered (ET) systems. The reason being that, comparative

evaluation of the two systems show, the TT-systems to have a better predictability,

testability and extensibility [10].

While going through the original implementation of leJOS we noticed that the system

lends itself more to a TT implementation than ET implementation. There are fixed

number of threads and a static schedule for each of them can be easily worked out. Now

these threads instead of busy-waiting will just have to wake up only at their observation

or action lattices (the periodic sequence of time points at which an entity is observed or

some action is performed)[10]. The time of deduction of any event is limited to the

distance between the lattice points and with a detailed schedule of the temporal behavior

of each thread the behavior of the system in the time domain can be easily predicted.

A native thread in leJOS polls the sensors every 3ms. When it has detected a change in

state it wakes up a listener thread, the listener thread in turn invokes associated methods

27

in the registered listener objects. Therefore no sensor events arrive faster than once in

3ms. Our Arbitrator uses this fact to implement the TT design, the primary thread wakes

up every 3ms and works through the takeControl() methods of each behavior to see if any

of them is ready.

On testing Tippy Senior, we found that the counters in the rotation sensor objects are

incremented no faster than once every 50ms. In our RotationNavigator the SteerThread

wakes up once every 30ms (adding some safety margin to compensate for the gear train

used in Tippy), reads the rotation sensor counters and checks the following

Whether the robot has traveled the specified distance or rotated through the

specified angle, if so it performs a normal stop operation.

public void start() { while(true) { // Check through all behavior.takeControl() starting at highest level behavior for(int i = totalBehaviors; i >= 0; --i) {

if (behavior[i].takeControl()) { if ((i > currentBehavior) || (actionThread.current == NONE))

{ if (i > currentBehavior)

behavior[currentBehavior].suppress(); currentBehavior = i; actionThread.execute(i); break; } }

} synchronized(actionSleep) {

try { actionSleep.wait(3); }catch(InterruptedException e){}

} } }

Figure 35: Arbitrator, TT implementation

28

Whether the robot is moving in a straight direction and floats or restarts a motor

to ensure this.

All the threads communicate with each other using shared variables and locks carefully

protect critical sections. This implementation therefore eliminates busy-waiting, deadlock

and other minor synchronization issues that occurred in the original implementation. The

Timer problem was handled just as in the ET implementation.

public void run() { while(true) {

synchronized (this) { if(command == ROTATE) checkRotate(); else if(command == SLROTATE) checkSlowRotate(); else if(command == FORWARD) checkForward(); else if(command == BACKWARD) checkBackward(); } synchronized (steerSleep) { try{ steerSleep.wait(30); }catch(InterruptedException e){} }

} }

Figure 36: RotationNavigator, TT implementation

29

4 Performance and Accuracy

The new implementations, both TT and ET, were uploaded onto Tippy Senior and test

runs were conducted. In both cases the robot completed its run without any errors. The

accuracy with which Tippy returned exactly to its home was around 70%. The reason

being the robot is still plagued by some nonsystematic errors. Any unevenness in the

floor makes Tippy deviate from straight line. Some amount of wheel slippage is also

present. Any collision causes the internal angle maintained to differ from the actual

angle, and the accuracy decreases as collision or the distance traveled increases [1].

Testing was also done with robots other than Tippy Senior. A Line Follower robot that

uses light sensor and rotation sensors was then run using the new implementations of

leJOS with success.

30

5 Conclusion

As the new implementations maintain the same interface as the API of the original

implementation, the existing application programs using leJOS API can be made to work

reliably by replacing the original classes with the newly implemented ones. The author of

[1], Brian Bagnall, will be incorporating the new implementation of the Arbitrator,

RotationNavigator and Timer in the next release of leJOS. Also, the example of leJOS

has helped us in gaining an insight into the pitfalls of concurrent programming and the

use of properly synchronized collaborating threads in the development of error-free and

efficient multi-threaded programs.

31

6 References

[1] Bagnall, B. Core Lego Mindstorms Programming. Prentice Hall PTR, 2002

[2] http://mindstorms.lego.com

[3] Brooks, R. A. "A Robust Layered Control System for a Mobile Robot", IEEE Journal

of Robotics and Automation, Vol. 2, No. 1, March 1986, pp. 14–23 ; also MIT AI Memo

864, September 1985.

[4] LeBouthillier, Arthur. Trends in Robot Control Architectures

-http://www.cyberg8t.com/pendragn/trends.htm

[5] Roston G.P and Krotkov, E., Dead Reckoning Navigation for Walking Robots, tech.

report CMU-RI-TR-91-27, Robotics Institute, Carnegie Mellon University, November,

1991.

[6] Amidi, O. Integrated Mobile Robot Control, tech. report CMU-RI-TR-90-17,

Robotics Institute, Carnegie Mellon University, May, 1990.

[7] http://www.phtpr.com/bagnall

[8] Andrews, G.R. Concurrent Programming: Principles and Practice.

Benjamin/Cummings,

Redwood City, CA, 1991, 656 pp.

[9] Mizuno, M. “ A Pattern-Based Methodology to develop Concurrent Programs”,

[10] Kopetz, H. “Should Responsive systems be event-triggered or time-triggered?”

IEICE Trans. Inf. and Syst., Vol E76-D(11);1325-1332, 1993.

[11] http://lejos.sourceforge.net

32

Appendix A: leJOS original implementation, version 2.1.0

Poll.java

/** * RCX access classes. */package josx.platform.rcx;

/** * Provides blocking access to events from the RCX. Poll is a bit * of a misnomer (since you don't 'poll' at all) but it takes its * name from the Unix call of the same name. */public class Poll{ public static final short SENSOR1_MASK = 0x01; public static final short SENSOR2_MASK = 0x02; public static final short SENSOR3_MASK = 0x04; public static final short ALL_SENSORS = 0x07;

public static final short RUN_MASK = 0x08; public static final short VIEW_MASK = 0x10; public static final short PRGM_MASK = 0x20; public static final short ALL_BUTTONS = 0x38; public static final short BUTTON_MASK_SHIFT = 3;

public static final short SERIAL_MASK = 0x40; public static final short SERIAL_SHIFT = 6;

private static Poll monitor = new Poll(true); // This is reflected in the kernel structure private short changed; // The 'changed' mask.

/** * Private constructor. Sets up the poller in the kernel. */ private Poll(boolean dummy) { setPoller(); } /** * Constructor. */ public Poll() { } /** * Wait for the sensor/button values to change then return. * * @param mask bit mask of values to monitor. * @param millis wait for at most millis milliseconds. 0 = forever. * @return a bit mask of the values that have changed * @throws InterruptedException

33

*/ public final int poll(int mask, int millis) throws InterruptedException { synchronized (monitor) { int ret = mask & monitor.changed; // The inputs we're interested in may have already changed // since we last looked so check before we wait. while (ret == 0) { monitor.wait(millis); // Work out what's changed that we're interested in. ret = mask & monitor.changed; }

// Clear the bits that we're monitoring. If anyone else // is also monitoring these bits its tough. monitor.changed &= ~mask; return ret; } }

/** * Set a throttle on the regularity with which inputs * are polled. * @param throttle number of sensor reads between polls. * Default value is 1. 0 means poll as often as possible. * Sensor reads occur every 3ms. */ public native final void setThrottle(int throttle);

/** * Sets up and starts the the poller in the kernel. */ private native final void setPoller();/* { new Thread() { public void run() { do { synchronized (monitor) { if (anthing has changed) { monitor.changed = whatever has changed notifyAll(); } } } while (true); } }.start(); }*/}

34

ListenerThread.java

package josx.platform.rcx;

/** * Utility class for dispatching events to button, sensor and serial listeners. * * @author Paul Andrews */class ListenerThread extends Thread{ static ListenerThread singleton = new ListenerThread(); private static final int MAX_LISTENER_CALLERS = 7; private static int [] masks; private static ListenerCaller [] listenerCallers; private static int numLC = 0;

int mask; Poll poller = new Poll();

static ListenerThread get() { synchronized (singleton) { if (!singleton.isAlive()) { masks = new int[MAX_LISTENER_CALLERS]; listenerCallers = new ListenerCaller[MAX_LISTENER_CALLERS]; singleton.setDaemon(true); singleton.setPriority(Thread.MAX_PRIORITY); singleton.start(); } } return singleton; }

void addToMask(int mask, ListenerCaller lc) { int i; this.mask |= mask;

for(i=0;i<numLC;i++) if (listenerCallers[i] == lc) break; if (i == numLC) { masks[numLC] = mask; listenerCallers[numLC++] = lc; } // Interrupt the polling thread, not the current one! interrupt(); }

void addButtonToMask(int id, ListenerCaller lc) { addToMask(id << Poll.BUTTON_MASK_SHIFT, lc); }

35

void addSensorToMask(int id, ListenerCaller lc) { addToMask(1 << id, lc); }

void addSerialToMask(ListenerCaller lc) { addToMask(1 << Poll.SERIAL_SHIFT, lc); } public void run() { for (;;) { try { int changed = poller.poll(mask, 0);

for(int i=0;i<numLC;i++) if ((changed & masks[i]) != 0) listenerCallers[i].callListeners();

} catch (InterruptedException ie) { } } }}

ListenerCaller.java


/** * Interface for calling calling lejos listeners. */public interface ListenerCaller { void callListeners();}

SensorListener.java


/** * Listener of sensor events. * @see josx.platform.rcx.Sensor#addSensorListener */public interface SensorListener{ /** * Called when the canonical value of the sensor changes. * @param aSource The sensor that generated the event. * @param aOldValue The old sensor value. * @param aNewValue The new sensor value. */ public void stateChanged (Sensor aSource, int aOldValue, int aNewValue);}

36

Sensor.java


/** * Abstraction for a sensor (considerably changed since alpha5). * There are three Sensor instances available: Sensor.S1, Sensor.S2 and * Sensor.S3. They correspond to sensor inputs labeled 1, 2 and 3 in the * RCX, respectively. Before using a sensor, you should set its mode * and type with <code>setTypeAndMode</code> using constants defined in <code>SensorConstants</code>. * You should also activate the sensor. * * You can poll for sensor values in a loop using the readValue method * or one of the other read methods. There is also a low level method which * can be used when maximum performance is required. Another way to * monitor sensor values is to add a <code>SensorListener</code>. All sensor events * are dispatched to listeners by a single thread created by this class. The * thread is a daemon thread and so will not prevent termination of an * application if all other threads have exited. * * Example: * <code><pre> * Sensor.S1.setTypeAndMode (3, 0x80); * Sensor.S1.activate(); * Sensor.S1.addSensorListener (new SensorListener() { * public void stateChanged (Sensor src, int oldValue, int newValue) { * // Will be called whenever sensor value changes * LCD.showNumber (newValue); * try { * Thread.sleep (100); * } catch (InterruptedException e) { * // ignore * } * } * }); * * </pre></code> * * @see josx.platform.rcx.SensorConstants * @see josx.platform.rcx.SensorListener */public class Sensor implements ListenerCaller{ private int iSensorId; private short iNumListeners = 0; private SensorListener[] iListeners; private int iPreviousValue; /**

37

* Sensor labeled 1 on RCX. */ public static final Sensor S1 = new Sensor (0);

/** * Sensor labeled 2 on RCX. */ public static final Sensor S2 = new Sensor (1); /** * Sensor labeled 3 on RCX. */ public static final Sensor S3 = new Sensor (2);

/** * Array containing all three sensors [0..2]. */ public static final Sensor[] SENSORS = { Sensor.S1, Sensor.S2, Sensor.S3 };

/** * Reads the canonical value of the sensor. */ public final int readValue() { return readSensorValue (iSensorId, 1); }

/** * Reads the raw value of the sensor. */ public final int readRawValue() { return readSensorValue (iSensorId, 0); }

/** * Reads the boolean value of the sensor. */ public final boolean readBooleanValue() { return readSensorValue (iSensorId, 2) != 0; }

private Sensor (int aId) { iSensorId = aId; setTypeAndMode (3, 0x80); }

/** * Return the ID of the sensor. One of 0, 1 or 2. */ public final int getId() { return iSensorId; }

38

/** * Adds a sensor listener. * * * NOTE 1: You can add at most 8 listeners. * NOTE 2: Synchronizing inside listener methods could result * in a deadlock. * * @see josx.platform.rcx.SensorListener */ public synchronized void addSensorListener (SensorListener aListener) { if (iListeners == null) { iListeners = new SensorListener[8]; } iListeners[iNumListeners++] = aListener; ListenerThread.get().addSensorToMask(iSensorId, this); }

/** * Activates the sensor. This method should be called * if you want to get accurate values from the * sensor. In the case of light sensors, you should see * the led go on when you call this method. */ public final void activate() { ROM.call ((short) 0x1946, (short) (0x1000 + iSensorId)); }

/** * Passivates the sensor. */ public final void passivate() { ROM.call ((short) 0x19C4, (short) (0x1000 + iSensorId)); }

/** * Sets the sensor's mode and type. If this method isn't called, * the default type is 3 (LIGHT) and the default mode is 0x80 (PERCENT). * @param aType 0 = RAW, 1 = TOUCH, 2 = TEMP, 3 = LIGHT, 4 = ROT. * @param aMode 0x00 = RAW, 0x20 = BOOL, 0x40 = EDGE, 0x60 = PULSE, 0x80 = PERCENT, * 0xA0 = DEGC, * 0xC0 = DEGF, 0xE0 = ANGLE. Also, mode can be OR'd with slope (0..31). * @see josx.platform.rcx.SensorConstants */ public final void setTypeAndMode (int aType, int aMode) { setSensorValue (iSensorId, aType, 1); setSensorValue (iSensorId, aMode, 0); }

39

/** * Resets the canonical sensor value. This may be useful for rotation sensors. */ public final void setPreviousValue (int aValue) { setSensorValue (iSensorId, aValue, 2); } /** * Low-level API for reading sensor values. * @param aSensorId Sensor ID (0..2). * @param aRequestType 0 = raw value, 1 = canonical value, 2 = boolean value. */ public static native int readSensorValue (int aSensorId, int aRequestType); private static native void setSensorValue (int aSensorId, int aVal, int aRequestType); public synchronized void callListeners() { int newValue = readSensorValue( iSensorId, 1); for (int i = 0; i < iNumListeners; i++) { iListeners[i].stateChanged( this, iPreviousValue, newValue); } iPreviousValue = newValue; }}

SensorConstants.java


/** * Constants for Sensor methods. * @see josx.platform.rcx.Sensor#setTypeAndMode */public interface SensorConstants{

public static final int SENSOR_TYPE_RAW = 0;public static final int SENSOR_TYPE_TOUCH = 1;public static final int SENSOR_TYPE_TEMP = 2;public static final int SENSOR_TYPE_LIGHT = 3;public static final int SENSOR_TYPE_ROT = 4;

public static final int SENSOR_MODE_RAW = 0x00;public static final int SENSOR_MODE_BOOL = 0x20;public static final int SENSOR_MODE_EDGE = 0x40;public static final int SENSOR_MODE_PULSE = 0x60;public static final int SENSOR_MODE_PCT = 0x80;public static final int SENSOR_MODE_DEGC = 0xa0;public static final int SENSOR_MODE_DEGF = 0xc0;public static final int SENSOR_MODE_ANGLE = 0xe0;

40

public static final int RAW_VALUE = 0;public static final int CANONICAL_VALUE = 1;public static final int BOOLEAN_VALUE = 2;

}

Behavior.java

package josx.robotics;

/*** The Behavior interface represents an object embodying a specific* behavior belonging to a robot. Each behavior must define three things: * 1) The circumstances to make this behavior seize control of the robot.* e.g. When the touch sensor determines the robot has collided with an object. * 2) The action to exhibit when this behavior takes control. * e.g. Back up and turn. * 3) The actions to perform when another behavior has seized control from this* behavior. * e.g. Stop the current movement and update coordinates. * These are represented by defining the methods takeControl(), action(),* and suppress() respectively. * A behavior control system has one or more Behavior objects. When you have defined* these objects, create an array of them and use that array to initialize an* Arbitrator object.** @see Arbitrator* @author <a href="mailto:[email protected]">Brian Bagnall</a>* @version 0.1 27-July-2001*/public interface Behavior { /** * Returns a boolean to indicate if this behavior should seize control of the robot. * For example, a robot that reacts if a touch sensor is pressed: * public boolean takeControl() { * return Sensor.S1.readBooleanValue(); * } * @return boolean Indicates if this Behavior should seize control. */ public boolean takeControl(); /** * The code in action() represents the actual action of the robot when this * behavior becomes active. It can be as complex as navigating around a * room, or as simple as playing a tune. * The contract for implementing this method is: * Any action can be started in this method. This method should not start a

41

* never ending loop. This method can return on its own, or when the suppress() * method is called; but it must return eventually. The action can run in * a seperate thread if the designer wishes it, and can therefore continue * running after this method call returns. */ public void action(); /** * The code in suppress() should stop the current behavior. This can include * stopping motors, or even calling methods to update internal data (such * as navigational coordinates). * The contract for implementing this method is: * This method will stop the action running in this Behavior class. This method * will not return until that action has been stopped. It is acceptable for a * delay to occur while the action() method finishes up. */ public void suppress(); }

Arbitrator.java


/*** Arbitrator controls which behavior should currently be active in * a behavior control system. Make sure to call start() after the * Arbitrator is instantiated.* @see Behavior* @author <a href="mailto:[email protected]">Brian Bagnall</a>* @version 0.1 27-July-2001*/public class Arbitrator { private Behavior [] behavior; private final int NONE = 99; private int currentBehavior; private BehaviorAction actionThread; /** * Allocates an Arbitrator object and initializes it with an array of * Behavior objects. The highest index in the Behavior array will have the * highest order behavior level, and hence will suppress all lower level * behaviors if it becomes active. The Behaviors in an Arbitrator can not * be changed once the arbitrator is initialized. 

42

* NOTE: Once the Arbitrator is initialized, the method start() must be * called to begin the arbitration. * @param behavior An array of Behavior objects. */ public Arbitrator(Behavior [] behaviors) { this.behavior = behaviors; currentBehavior = NONE; actionThread = new BehaviorAction(); actionThread.start(); } /** * This method starts the arbitration of Behaviors. * Modifying the start() method is not recomended. * Note: Arbitrator does not run in a seperate thread, and hence the start() * method will never return. */ public void start() { int totalBehaviors = behavior.length - 1;

while(true) { // Check through all behavior.takeControl() starting at highest level behavior for(int i = totalBehaviors;i>=0;--i) { if(behavior[i].takeControl()) { // As soon as takeControl() is true, execute the currentBehavior.suppress() //if(behavior[i] != currentBehavior) { if(i != currentBehavior) { // Prevents program from running same action over and over again if (currentBehavior != NONE) { if(currentBehavior >= i) // If higher level thread, wait to complete.. while(!actionThread.done) {Thread.yield();} behavior[currentBehavior].suppress(); } // Make currentBehavior this one currentBehavior = i;

// Run the currentBehavior.behaviorAction() actionThread.execute(i); Thread.yield(); } break; // Breaks out of for() loop } } } }

/** * This class handles the action() methods of the Behaviors. */ private class BehaviorAction extends Thread { public boolean done = true; int current = NONE;

43

public void run() { while(true) { synchronized(this) { if(current != NONE) { done = false; behavior[current].action(); current = NONE; done = true; } } Thread.yield(); } } public synchronized void execute(int index) { current = index; } }}

Navigator.java


import josx.platform.rcx.*;

/** * The Navigator interface contains methods for performing basic navigational * movements. Normally the Navigator class is instantiated as an object and * methods are called on that object. * * Note: This class will only work for robots using two motors to steer differentially * that can rotate within its footprint (i.e. turn on one spot). * * @author <a href="mailto:[email protected]">Brian Bagnall</a> * @version 0.1 23-June-2001 */public interface Navigator{ /** * Returns the current x coordinate of the RCX. * Note: At present it will only give an updated reading when the RCX is stopped. * @return float Present x coordinate. */ public float getX();

44

/** * Returns the current y coordinate of the RCX. * Note: At present it will only give an updated reading when the RCX is stopped. * @return float Present y coordinate. */ public float getY(); /** * Returns the current angle the RCX robot is facing. * Note: At present it will only give an updated reading when the RCX is stopped. * @return float Angle in degrees. */ public float getAngle(); /** * Rotates the RCX robot a specific number of degrees in a direction (+ or -).This * method will return once the rotation is complete. * * @param angle Angle to rotate in degrees. A positive value rotates left, a negative value right. */ public void rotate(float angle);

/** * Rotates the RCX robot to point in a certain direction. It will take the shortest * path necessary to point to the desired angle. Method returns once rotation is complete. * * @param angle The angle to rotate to, in degrees. */ public void gotoAngle(float angle);

/** * Rotates the RCX robot towards the target point and moves the required distance. * * @param x The x coordinate to move to. * @param y The y coordinate to move to. */ public void gotoPoint(float x, float y);

/** * Moves the RCX robot a specific distance. A positive value moves it forward and * a negative value moves it backward. Method returns when movement is done. * * @param distance The positive or negative distance to move the robot. */ public void travel(int distance); /**

45

* Moves the RCX robot forward until stop() is called. * * @see Navigator#stop(). */ public void forward();

/** * Moves the RCX robot backward until stop() is called. * * @see Navigator#stop(). */ public void backward();

/** * Halts the RCX robot and calculates new x, y coordinates. * * @see Navigator#forward(). */ public void stop();}

RotationNavigator.java


import josx.platform.rcx.*;import josx.util.*;

// !! For rotation, it should straighten out when stopped to try to keep// the angle somewhat accurate.

// !! Should all methods call stop() first in case it was roaming?// OR methods account for RCX currently in moving mode?

// !! All methods that change x, y must be synchronized.

/** * The RotationNavigator class contains methods for performing basic navigational * movements. This class uses two rotation sensors to monitor the wheels of the * differential drive. For this class to work properly, the rotation sensors * should record positive (+) values when the wheels move forward, and negative (-) * values when the wheels move backward. This class also assumes the Motor.forward() * command will cause the drive wheels to move in a forward direction. * Note: This class will only work for robots using two motors to steer differentially * that can rotate within its footprint (i.e. turn on one spot). * * @author <a href="mailto:[email protected]">Brian Bagnall</a> * @version 0.1 19-August-2001 */

46

public class RotationNavigator implements Navigator, SensorConstants {

// orientation and co-ordinate data private float angle; private float x; private float y;

// Motors for differential steering: private Motor left; private Motor right; // Rotation sensors: private Sensor rotLeft; private Sensor rotRight; // Movement values: private float COUNTS_PER_CM; private float COUNTS_PER_DEGREE; // Internal states private boolean moving; private byte command; // command constants: private byte STOP = 0; private byte FORWARD = 1; private byte BACKWARD = 2; private byte LEFT_ROTATE = 3; // Unused private byte RIGHT_ROTATE = 4; // Unused /** * Allocates a RotationNavigator object and initializes if with the proper motors and sensors. * The x and y values will each equal 0 (cm's) on initialization, and the starting * angle is 0 degrees, so if the first move is forward() the robot will run along * the x axis. * Note: If you find your robot is going backwards or in circles when you tell it to go forwards, try * rotating the wires to the motor ports by 90 or 180 degrees. * @param wheelDiameter The diameter of the wheel, usually printed right on the * wheel, in centimeters (e.g. 49.6 mm = 4.96 cm) * @param driveLength The distance from the center of the left tire to the center * of the right tire, in centimeters. * @param ratio The ratio of sensor rotations to wheel rotations. * e.g. 3 complete rotations of the sensor for every one turn of the wheel = 3f * 1 rotation of the sensor for every 2 turns of the wheel = 0.5f * @param rightMotor The motor used to drive the right wheel e.g. Motor.C. * @param leftMotor The motor used to drive the left wheel e.g. Motor.A. * @param rightRot Sensor used to read rotations from the right wheel. e.g. Sensor.S3

47

* @param leftRot Sensor used to read rotations from the left wheel. e.g. Sensor.S1 */ public RotationNavigator(float wheelDiameter, float driveLength, float ratio, Motor leftMotor, Motor rightMotor, Sensor leftRot, Sensor rightRot) { this.right = rightMotor; this.left = leftMotor;

this.rotLeft = leftRot; this.rotLeft.setTypeAndMode(SENSOR_TYPE_ROT, SENSOR_MODE_ANGLE); this.rotLeft.setPreviousValue(0); this.rotLeft.activate(); this.rotRight = rightRot; this.rotRight.setTypeAndMode(SENSOR_TYPE_ROT, SENSOR_MODE_ANGLE); this.rotRight.setPreviousValue(0); this.rotRight.activate(); // Set coordinates and starting angle: angle = 0.0f; x = 0.0f; y = 0.0f; moving = false; command = STOP; // Calculate the counts per centimeter float wheelCircumference = wheelDiameter * (float)Math.PI; COUNTS_PER_CM = (16 * ratio)/wheelCircumference; // Calculate counts per degree float fullRotation = (driveLength * (float)Math.PI); COUNTS_PER_DEGREE = ((fullRotation/wheelCircumference)*(16*ratio))/360; // Thread is for keeping the RCX straight when driving by // monitoring the rotation sensors while moving. SteerThread steering = new SteerThread(); steering.start(); } /** * Overloaded RotationNavigator constructor that assumes the following: * Left motor = Motor.A Right motor = Motor.C * Left rotation sensor = Sensor.S1 Right rotation sensor = Sensor.S3 * @param wheelDiameter The diameter of the wheel, usually printed right on the * wheel, in centimeters (e.g. 49.6 mm = 4.96 cm) * @param axleLength The distance from the center of the left tire to the center * of the right tire, in centimeters. * @param ratio The ratio of sensor rotations to wheel rotations. 

48

* e.g. 3 complete rotations of the sensor for every one turn of the wheel = 3f * 1 rotation of the sensor for every 2 turns of the wheel = 0.5f */ public RotationNavigator(float wheelDiameter, float driveLength, float ratio) { this(wheelDiameter, driveLength, ratio, Motor.A, Motor.C, Sensor.S1, Sensor.S3); } /** * Returns the current x coordinate of the RCX. * Note: At present it will only give an updated reading when the RCX is stopped. * @return float Present x coordinate. */ public float getX() { // !! In future, if RCX is on the move it should return the present calculation of x return x; } /** * Returns the current y coordinate of the RCX. * Note: At present it will only give an updated reading when the RCX is stopped. * @return float Present y coordinate. */ public float getY() { return y; }

/** * Returns the current angle the RCX robot is facing. * Note: At present it will only give an updated reading when the RCX is stopped. * @return float Angle in degrees. */ public float getAngle() { return angle; }

/** * Rotates the RCX robot a specific number of degrees in a direction (+ or -). * This method will return once the rotation is complete. * * @param angle Angle to rotate in degrees. A positive value rotates left, a negative value right. */ public void rotate(float angle) { // keep track of angle this.angle = this.angle + angle; this.angle = (int)this.angle % 360; // Must be < 360 degrees // Is it possible to do the following with modulo (%) ???

49

while(this.angle < 0) this.angle += 360; // Must be > 0 // Calculate the number of intervals of rotation sensor to count int count = (int)(COUNTS_PER_DEGREE * angle); rotLeft.setPreviousValue(0); rotRight.setPreviousValue(0); if (angle > 0) { right.forward(); left.backward(); while(rotLeft.readValue() > (count*-1) || rotRight.readValue() < count) {} } else if (angle < 0) { right.backward(); left.forward(); while(rotLeft.readValue() < (count*-1) || rotRight.readValue() > count) {} }

right.stop(); left.stop(); }

/** * Rotates the RCX robot to point in a certain direction. It will take the shortest * path necessary to point to the desired angle. Method returns once rotation is complete. * * @param angle The angle to rotate to, in degrees. */ public void gotoAngle(float angle) { // in future, use modulo instead of while loop??? float difference = angle - this.angle; while(difference > 180) difference = difference - 360; // shortest path to goal angle while(difference < -180) difference = difference + 360; // shortest path to goal angle rotate(difference); }

/** * Rotates the RCX robot towards the target point and moves the required distance. * * @param x The x coordinate to move to. * @param y The y coordinate to move to. */ public void gotoPoint(float x, float y) {

// Determine relative points float x1 = x - this.x; float y1 = y - this.y;

// Calculate angle to go to:

50

float angle = (float)Math.atan2(y1,x1);

// Calculate distance to travel: float distance; if(y1 != 0) distance = y1/(float)Math.sin(angle); else distance = x1/(float)Math.cos(angle);

// Convert angle from rads to degrees: angle = (float)Math.toDegrees(angle); // Now convert theory into action: gotoAngle(angle); travel(Math.round(distance)); }

/** * Moves the RCX robot a specific distance. A positive value moves it forwards and * a negative value moves it backwards. Method returns when movement is done. * * @param dist The positive or negative distance to move the robot (in centimeters). */ public void travel(int dist) { // !! The command != STOP lines need to be tested!!! // !! Should stop and exit travel() when travel() interrupted by stop() int counts = (int)(dist * COUNTS_PER_CM); if(dist > 0) { forward(); while(command != STOP && (rotLeft.readValue() < counts || rotRight.readValue() < counts)) { Thread.yield(); } } else if(dist < 0) { backward(); while(command != STOP && rotLeft.readValue() > counts || rotRight.readValue() > counts) { Thread.yield(); } } stop(); }

/** * Moves the RCX robot forward until stop() is called. * * @see Navigator#stop(). */ public void forward() { rotLeft.setPreviousValue(0); rotRight.setPreviousValue(0);

51

command = FORWARD; }

/** * Inner class that monitors the rotation sensors and keeps the * navigator steering straight. */ private class SteerThread extends Thread { public void run() { while(true) { while(command == FORWARD) { left.forward(); right.forward(); moving = true; if(rotLeft.readValue() > rotRight.readValue()) { left.flt(); while(rotLeft.readValue() > rotRight.readValue()) {} left.forward(); } if(rotRight.readValue() > rotLeft.readValue()) { right.flt(); while(rotRight.readValue() > rotLeft.readValue()) {} right.forward(); } Thread.yield(); } while(command == BACKWARD) { left.backward(); right.backward(); moving = true; if(rotLeft.readValue() < rotRight.readValue()) { left.flt(); while(rotLeft.readValue() < rotRight.readValue()) {} left.backward(); } if(rotRight.readValue() < rotLeft.readValue()) { right.flt(); while(rotRight.readValue() < rotLeft.readValue()) {} right.backward(); } Thread.yield(); } moving = false; Thread.yield(); } } } /** * Moves the RCX robot backward until stop() is called. * * @see Navigator#stop(). */ public void backward() {

52

rotLeft.setPreviousValue(0); rotRight.setPreviousValue(0); command = BACKWARD; }

/** * Halts the RCX robot and calculates new x, y coordinates. * * @see Navigator#forward(). */ public void stop() { if(moving) { command = STOP; while(moving) { Thread.yield(); } left.stop(); right.stop(); // Recalculate x-y coordinates based on rotation sensors int rotAvg = (rotLeft.readValue() + rotRight.readValue()) / 2; float centimeters = rotAvg / COUNTS_PER_CM; // update x, y coordinates x = x + (float)(Math.cos(Math.toRadians(angle)) * centimeters); y = y + (float)(Math.sin(Math.toRadians(angle)) * centimeters); } }}

53

Appendix B: Event Triggered Implementation

Arbitrator.java

// package josx.robotics;import josx.robotics.*;import josx.platform.rcx.*;/*** Arbitrator controls which behavior should currently be active in* a behavior control system. Make sure to call start() after the* Arbitrator is instantiated.* @see Behavior* @version 0.21 28-April-2003*/

public class Arbitrator { private Behavior [] behavior; private final int NONE = 99; private int totalBehaviors; private int currentBehavior; private BehaviorAction actionThread; private Object arbitratorLock; private int in,out;

/** * Allocates an Arbitrator object and initializes it with an array of * Behavior objects. The highest index in the Behavior array will have the * highest order behavior level, and hence will suppress all lower level * behaviors if it becomes active. The Behaviors in an Arbitrator can not * be changed once the arbitrator is initialized. * NOTE: Once the Arbitrator is initialized, the method start() must be * called to begin the arbitration. * @param behavior An array of Behavior objects. */

public Arbitrator() {}

public void initArbitrator(Behavior [] behaviors) { this.behavior = behaviors; totalBehaviors = behavior.length - 1; currentBehavior = NONE;

actionThread = new BehaviorAction();arbitratorLock = new Object();

actionThread.start(); in = out = 0;

}

public void wakeUpArbitrator() { synchronized(arbitratorLock) {

54

in++; arbitratorLock.notify();

} }

/** * This method starts the arbitration of Behaviors. * Modifying the start() method is not recomended. * Note: Arbitrator does not run in a seperate thread, and hence the start() * method will never return. */ public void start(Behavior [] behaviors) {

boolean waited = false;initArbitrator(behaviors);

while(true) { // Check through all behavior.takeControl() starting at highest level behavior for(int i = totalBehaviors; i >= 0; --i) {

if (behavior[i].takeControl()) { if ((i > currentBehavior) || (actionThread.current ==

NONE)) {

if (i > currentBehavior) behavior[currentBehavior].suppress();

currentBehavior = i; actionThread.execute(i); break; }

} } synchronized(arbitratorLock) {

try { while (in <= out) {arbitratorLock.wait(); waited =

true;} out++; }catch(InterruptedException e){}

} if (! waited) Thread.yield(); // if it did not sleep, it may be better to give// other threads a chance to run, rather than// this thread keeps running.

else waited = false; }

} /** * This class handles the action() methods of the Behaviors. */ private class BehaviorAction extends Thread { int current = NONE; int i;

public void run() {while(true) {

55

synchronized(this) { if (current != NONE) {

LCD.showNumber(current); LCD.refresh(); behavior[current].action(); current = NONE; wakeUpArbitrator(); } } Thread.yield(); // since Action thraead is not time

triggered, it is necessary to yield.}

}

public synchronized void execute(int index) { current = index; } }}

RotationNavigator.java

//package josx.robotics;import josx.robotics.*;

import josx.platform.rcx.*;import josx.util.*;

public class RotationNavigator extends Thread implements Navigator, SensorConstants, SensorListener {

// orientation and co-ordinate data private float angle; private float rotAngle; private float x; private float y;

// Motors for differential steering: private Motor left; private Motor right;

// Rotation sensors: private Sensor rotLeft; private Sensor rotRight;

// Movement values: private int count; private float COUNTS_PER_CM; private float COUNTS_PER_DEGREE;

// Internal states private byte command; private boolean leftFlt,rightFlt; private Object steerLock; private int in, out;

56

private int oldLeftValue, oldRightValue;

// command constants: private final byte NONE = 0; private final byte STOP = 1; private final byte FORWARD = 2; private final byte BACKWARD = 3; private final byte ROTATE = 4; private final byte SLROTATE = 5;

// Object used for synchronizing behaviorAction and SteerThread: Object steerNav = new Object();

private int callLevel = 0;

//To skip all actions and force behavior change

/** * Allocates a RotationNavigator object and initializes if with the proper motors and sensors. * The x and y values will each equal 0 (cm's) on initialization, and the starting * angle is 0 degrees, so if the first move is forward() the robot will run along * the x axis. * Note: If you find your robot is going backwards or in circles when you tell it to go forwards, try * rotating the wires to the motor ports by 90 or 180 degrees. * @param wheelDiameter The diameter of the wheel, usually printed right on the * wheel, in centimeters (e.g. 49.6 mm = 4.96 cm) * @param driveLength The distance from the center of the left tire to the center * of the right tire, in centimeters. * @param ratio The ratio of sensor rotations to wheel rotations. * e.g. 3 complete rotations of the sensor for every one turn of the wheel = 3f * 1 rotation of the sensor for every 2 turns of the wheel = 0.5f * @param rightMotor The motor used to drive the right wheel e.g. Motor.C. * @param leftMotor The motor used to drive the left wheel e.g. Motor.A. * @param rightRot Sensor used to read rotations from the right wheel. e.g. Sensor.S3 * @param leftRot Sensor used to read rotations from the left wheel. e.g. Sensor.S1 */

public RotationNavigatorET(float wheelDiameter, float driveLength, float ratio, Motor leftMotor, Motor rightMotor, Sensor leftRot, Sensor rightRot) { this.right = rightMotor; this.left = leftMotor;

this.rotLeft = leftRot; this.rotLeft.setTypeAndMode(SENSOR_TYPE_ROT, SENSOR_MODE_ANGLE); this.rotLeft.setPreviousValue(0);

57

this.rotLeft.activate();

this.rotRight = rightRot; this.rotRight.setTypeAndMode(SENSOR_TYPE_ROT, SENSOR_MODE_ANGLE); this.rotRight.setPreviousValue(0); this.rotRight.activate();

// Set coordinates and starting angle: angle = 0.0f; x = 0.0f; y = 0.0f; rotAngle = 0.0f;

command = NONE; leftFlt=rightFlt=false;

// Calculate the counts per centimeter float wheelCircumference = wheelDiameter * (float)Math.PI; COUNTS_PER_CM = (16 * ratio)/wheelCircumference;

// Calculate counts per degree float fullRotation = (driveLength * (float)Math.PI); COUNTS_PER_DEGREE = ((fullRotation/wheelCircumference)*(16*ratio))/360; count=0; steerLock = new Object(); in = out = 0; Sensor.S1.addSensorListener (this); Sensor.S3.addSensorListener (this);

// Thread is for keeping the RCX straight when driving by // monitoring the rotation sensors while moving. this.start(); }

/** * Overloaded RotationNavigator constructor that assumes the following: * Left motor = Motor.A Right motor = Motor.C * Left rotation sensor = Sensor.S1 Right rotation sensor = Sensor.S3 * @param wheelDiameter The diameter of the wheel, usually printed right on the * wheel, in centimeters (e.g. 49.6 mm = 4.96 cm) * @param axleLength The distance from the center of the left tire to the center * of the right tire, in centimeters. * @param ratio The ratio of sensor rotations to wheel rotations. * e.g. 3 complete rotations of the sensor for every one turn of the wheel = 3f * 1 rotation of the sensor for every 2 turns of the wheel = 0.5f */ public RotationNavigatorET(float wheelDiameter, float driveLength, float ratio) { this(wheelDiameter, driveLength, ratio, Motor.A, Motor.C, Sensor.S1, Sensor.S3); }

58

/** * Returns the current x coordinate of the RCX. * Note: At present it will only give an updated reading when the RCX is stopped. * @return float Present x coordinate. */ public float getX() { // !! In future, if RCX is on the move it should return the present calculation of x return x; }

/** * Returns the current y coordinate of the RCX. * Note: At present it will only give an updated reading when the RCX is stopped. * @return float Present y coordinate. */ public float getY() { return y; }

/** * Returns the current angle the RCX robot is facing. * Note: At present it will only give an updated reading when the RCX is stopped. * @return float Angle in degrees. */ public float getAngle() { return angle; }

/** * Rotates the RCX robot a specific number of degrees in a direction (+ or -). * This method will return once the rotation is complete. * * @param angle Angle to rotate in degrees. A positive value rotates left, a negative value right. */ public synchronized void rotate(float angle) {

callLevel++;

if(command != STOP){ // Calculate the number of intervals of rotation sensor to count count = (int)(COUNTS_PER_DEGREE * angle); rotAngle=angle;

command=ROTATE;

rotLeft.setPreviousValue(0); rotRight.setPreviousValue(0);

if (angle > 0) { right.forward();

59

left.backward(); } else if (angle < 0) {

right.backward(); left.forward();

}

try{ this.wait(); }catch(InterruptedException e){}

//Update orientation // during sleep, stop() might be called. In such a case, updateAngle is called // from stop() if (command != STOP) updateAngle(); }

if ((--callLevel == 0) && (command == STOP)) resetStop(); }

public synchronized void slowRotate(float angle) {

callLevel++;

if(command != STOP){ // Calculate the number of intervals of rotation sensor to count count = (int)(COUNTS_PER_DEGREE * angle); rotAngle=angle;

command=SLROTATE;

rotLeft.setPreviousValue(0); rotRight.setPreviousValue(0);

if (angle > 0) { right.forward(); left.flt();

} else if (angle < 0) {

right.flt(); left.forward();

}

try{ this.wait(); }catch(InterruptedException e){}

//Update orientation // during sleep, stop() might be called. In such a case, updateAngle is called // from stop() if (command != STOP) updateAngle(); }

if ((--callLevel == 0) && (command == STOP)) resetStop();

60

}

/** * Rotates the RCX robot to point in a certain direction. It will take the shortest * path necessary to point to the desired angle. Method returns once rotation is complete. * * @param angle The angle to rotate to, in degrees. */ public void gotoAngle(float angle) {

callLevel++;

// in future, use modulo instead of while loop??? float difference = angle - this.angle; while(difference > 180) difference = difference - 360; // shortest path to goal angle while(difference < -180) difference = difference + 360; // shortest path to goal angle rotate(difference);

synchronized (this) { // just in case, this is called directly from outside if ((--callLevel == 0) && (command == STOP)) resetStop();

} }

/** * Rotates the RCX robot towards the target point and moves the required distance. * * @param x The x coordinate to move to. * @param y The y coordinate to move to. */ public void gotoPoint(float x, float y) {

callLevel++;

// Determine relative points float x1 = x - this.x; float y1 = y - this.y;

// Calculate angle to go to: float angle = (float)Math.atan2(y1,x1);

// Calculate distance to travel: float distance; if(y1 != 0) distance = y1/(float)Math.sin(angle); else distance = x1/(float)Math.cos(angle);

// Convert angle from rads to degrees: angle = (float)Math.toDegrees(angle);

// Now convert theory into action: gotoAngle(angle); travel(Math.round(distance));

61

synchronized (this) { // just in case, this is called directly from outside if ((--callLevel == 0) && (command == STOP)) resetStop();

} }

/** * Moves the RCX robot a specific distance. A positive value moves it forwards and * a negative value moves it backwards. Method returns when movement is done. * * @param dist The positive or negative distance to move the robot (in centimeters). */ public synchronized void travel(int dist) {

callLevel++;

if(command != STOP) { count = (int)(dist * COUNTS_PER_CM);

rotLeft.setPreviousValue(0); rotRight.setPreviousValue(0); leftFlt=rightFlt=false;

if(dist > 0)forward(); else backward();

// during sleep, stop() might be called. In such a case, updateCoordinate is called // from stop() if (command != STOP) updateCoordinate(); }


/** * Moves the RCX robot forward until stop() is called. * * @see Navigator#stop(). */ public synchronized void forward() { callLevel++; if (callLevel==1) count=32768; left.forward(); right.forward(); command = FORWARD; try { this.wait(); // the action thread waits for normal completion or emergency stop } catch (InterruptedException e){}

62

public synchronized void backward() { callLevel++; if(callLevel==1) count=-32767; left.backward(); right.backward(); command = BACKWARD; try { this.wait(); // the action thread waits for normal completion or emergency stop } catch (InterruptedException e){}


private void updateCoordinate() { int rotAvg = (rotLeft.readValue() + rotRight.readValue()) / 2; float centimeters = rotAvg / COUNTS_PER_CM; // update x, y coordinates x = x + (float)(Math.cos(Math.toRadians(angle)) * centimeters); y = y + (float)(Math.sin(Math.toRadians(angle)) * centimeters); }

private void updateAngle() { // keep track of angle if (rotAngle > 0) this.angle += (int)(rotRight.readValue() / COUNTS_PER_DEGREE); else this.angle -= (int)(rotLeft.readValue() / COUNTS_PER_DEGREE); this.angle = (int)this.angle % 360; // Must be < 360 degrees

// Is it possible to do the following with modulo (%) ??? while(this.angle < 0) this.angle += 360; // Must be > 0 }

private void halt() { // inside "this" lock // handles normal termination of rotate, travel

right.stop(); left.stop(); command = NONE; this.notify(); // wake up the action thread

}

private void checkRotate() { // inside "this" lock if(rotAngle >= 0) {

if(rotLeft.readValue() <= (count*-1) && rotRight.readValue() >= count) halt();

} else {

63

if(rotLeft.readValue() >= (count*-1) && rotRight.readValue() <= count) halt(); } } private void checkSlowRotate() { // inside "this" lock if(rotAngle >= 0) {

if(rotRight.readValue() >= count) halt(); } else {

if(rotLeft.readValue() >= count) halt(); } } private void checkForward() { // inside "this" lock int lcount = rotLeft.readValue(); int rcount = rotRight.readValue();

if (lcount >= count && rcount >= count) halt(); else if (lcount > rcount){

if (!leftFlt && !rightFlt) {left.flt(); leftFlt = true;} else if (rightFlt) {right.forward(); rightFlt = false;}

} else if (lcount < rcount){ if (!rightFlt && !leftFlt) {right.flt(); rightFlt = true;} else if (leftFlt) {left.forward(); leftFlt = false;}

} else if (leftFlt) {left.forward(); leftFlt = false;} else if (rightFlt) {right.forward(); rightFlt = false;} }

private void checkBackward() { // inside "this" lock int lcount = rotLeft.readValue(); int rcount = rotRight.readValue();

if(lcount <= count && rcount <= count) halt(); else if (lcount > rcount) {

if (!rightFlt && !leftFlt) {right.flt(); rightFlt = true;} else if (leftFlt) {left.backward(); leftFlt = false;}

} else if (lcount < rcount) { if (!leftFlt && !rightFlt) {left.flt(); leftFlt = true;} else if (rightFlt) {right.backward(); rightFlt = false;}

} else if (leftFlt) {left.backward(); leftFlt = false;} else if (rightFlt) {right.backward(); rightFlt = false;} }

// Emergency stop() public synchronized void stop() {

left.stop();right.stop();if (command == ROTATE || command == SLROTATE) updateAngle();if ((command == FORWARD) || (command == BACKWARD))

updateCoordinate();command = STOP;this.notify();

}

// ater stop() is called, resetStop() must be called. // Does the callLevel implementation work reliably ???

64

private void resetStop(){ // already inside synchronized (this) LCD.showNumber(0); LCD.refresh(); command = NONE; }

public void stateChanged(Sensor rotation, int oldValue, int newValue) {

if (oldValue != newValue) {synchronized(steerLock) {

in++;steerLock.notify();

}}

}

// steering thread, wake up every 30 msec and check steering based on the rotation sensor

// readings public void run() {

boolean waited = false; while(true) {

synchronized (this) { if(command == ROTATE) checkRotate(); else if(command == SLROTATE) checkSlowRotate(); else if(command == FORWARD) checkForward(); else if(command == BACKWARD) checkBackward(); } synchronized (steerLock) { try{ while (in <= out) {steerLock.wait(); waited = true;} out++; }catch(InterruptedException e){} } if (! waited) Thread.yield();

// if it did not sleep, it may be better to give// other threads a chance to run, rather than// this thread keeps running.

else waited = false;

} }

}

Appendix C: Time Triggered Implementation

65

Arbitrator.java


import josx.robotics.*;import josx.platform.rcx.*;/*** Arbitrator controls which behavior should currently be active in* a behavior control system. Make sure to call start() after the* Arbitrator is instantiated.* @see Behavior* @version 0.21 28-April-2003*/public class Arbitrator {

private Behavior [] behavior; private final int NONE = 99; private int totalBehaviors; private int currentBehavior; private BehaviorAction actionThread; private Object actionSleep; /** * Allocates an Arbitrator object and initializes it with an array of * Behavior objects. The highest index in the Behavior array will have the * highest order behavior level, and hence will suppress all lower level * behaviors if it becomes active. The Behaviors in an Arbitrator can not * be changed once the arbitrator is initialized. * NOTE: Once the Arbitrator is initialized, the method start() must be * called to begin the arbitration. * @param behavior An array of Behavior objects. */ public Arbitrator(Behavior [] behaviors) { this.behavior = behaviors; currentBehavior = NONE; actionThread = new BehaviorAction(); actionSleep = new Object(); totalBehaviors = behavior.length - 1; actionThread.start(); }

/** * This method starts the arbitration of Behaviors. * Modifying the start() method is not recomended. * Note: Arbitrator does not run in a seperate thread, and hence the start() * method will never return. */ public void start() { while(true) { // Check through all behavior.takeControl() starting at highest level behavior

66

for(int i = totalBehaviors; i >= 0; --i) {if (behavior[i].takeControl()) { if ((i > currentBehavior) || (actionThread.current ==

NONE)) { if (i > currentBehavior)

behavior[currentBehavior].suppress(); currentBehavior = i; actionThread.execute(i); break; }

} } synchronized(actionSleep) {

try { actionSleep.wait(3); }catch(InterruptedException e){}

} } }

/** * This class handles the action() methods of the Behaviors. */ private class BehaviorAction extends Thread { int current = NONE; int i;

public void run() {while(true) { synchronized(this) { if (current != NONE) {

LCD.showNumber(current); LCD.refresh(); behavior[current].action(); current = NONE; } } Thread.yield(); // since Action thraead is not time

triggered, it is necessary to yield.}

}

public synchronized void execute(int index) { current = index; } }}

RotationNavigotor.java

Only the run method is given here, the other methods are just the same as their counterparts in Event Triggered Implementation

67

// steering thread, wake up every 30 msec and check steering based on the rotation sensor

// readings public void run() { while(true) {

synchronized (this) { if(command == ROTATE) checkRotate(); else if(command == SLROTATE) checkSlowRotate(); else if(command == FORWARD) checkForward(); else if(command == BACKWARD) checkBackward(); } synchronized (steerSleep) { try{ steerSleep.wait(30); }catch(InterruptedException e){} }

} }

Timer.java

import josx.util.*;public class Timer{ private TimerListener myListener; private Thread myThread ; private int delay ; private boolean running ;

/** * Create a Timer object. Every theDelay milliseconds * the el.timedOut() function is called. You may * change the delay with setDelay(int). You need * to call start() explicitly. */ public Timer(int theDelay, TimerListener el) {

running = false;delay = theDelay;myListener = el;

myThread = new Thread() { public void run() {

int d;boolean r;while(true) { synchronized(Timer.this) { d = delay; r = running; } if (r) { try

68

{ Thread.sleep (d);

myListener.timedOut(); } catch (InterruptedException e) {

// ignore }

} else { yield(); }}

}};

}

/** * access how man milliseconds between timedOut() messages. */ public synchronized int getDelay() {

return delay; } /** * Change the delay between timedOut messages. Safe to call * while start()ed. Time in milli-seconds. */ public synchronized void setDelay(int newDelay) {

delay = newDelay; }

/** * Stops the timer. timedOut() messages are not sent. */ public synchronized void stop() { try { myThread.interrupt();

running = false; }catch(Exception e){} }

/** * Starts the timer, telling it to send timeOut() methods * to the TimerListener. */ public synchronized void start() {

running = true;if (!myThread.isAlive()) myThread.start();

}}

69

COMPETING THREADS VS - People | Computer...

Documents

Transcript of COMPETING THREADS VS - People | Computer...