competing threads vs - people | computer...
TRANSCRIPT
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
v
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
14
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
package josx.platform.rcx;
/** * Interface for calling calling lejos listeners. */public interface ListenerCaller { void callListeners();}
SensorListener.java
package josx.platform.rcx;
/** * 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
package josx.platform.rcx;
/** * Abstraction for a sensor (<i>considerably changed since alpha5</i>). * 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. * <p> * 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. * <p> * Example:<p> * <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. * <p> * <b> * NOTE 1: You can add at most 8 listeners.<br> * NOTE 2: Synchronizing inside listener methods could result * in a deadlock. * </b> * @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); } /** * <i>Low-level API</i> 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
package josx.platform.rcx;
/** * 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: <BR>* 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.<BR>* 2) The action to exhibit when this behavior takes control. * e.g. Back up and turn.<BR>* 3) The actions to perform when another behavior has seized control from this* behavior. * e.g. Stop the current movement and update coordinates.<BR>* These are represented by defining the methods takeControl(), action(),* and suppress() respectively. <BR>* 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: <BR> * public boolean takeControl() { <BR> * return Sensor.S1.readBooleanValue(); <BR> * } <BR> * @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.<BR> * <B>The contract for implementing this method is:</B><BR> * 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). <BR> * <B>The contract for implementing this method is:</B><BR> * This method will stop the action running in this Behavior class. This method * will <I>not</I> 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
package josx.robotics;
/*** 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.<BR>
42
* <B>NOTE:</B> 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. <BR> * 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
package josx.robotics;
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
package josx.robotics;
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.<BR> * 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. <BR> * 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.<BR> * e.g. 3 complete rotations of the sensor for every one turn of the wheel = 3f<BR> * 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:<BR> * Left motor = Motor.A Right motor = Motor.C <BR> * 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.<BR>
48
* e.g. 3 complete rotations of the sensor for every one turn of the wheel = 3f<BR> * 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.<BR> * <B>NOTE:</B> 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. <BR> * 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. <BR> * 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.<BR> * e.g. 3 complete rotations of the sensor for every one turn of the wheel = 3f<BR> * 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:<BR> * Left motor = Motor.A Right motor = Motor.C <BR> * 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.<BR> * e.g. 3 complete rotations of the sensor for every one turn of the wheel = 3f<BR> * 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(); }
if ((--callLevel == 0) && (command == STOP)) resetStop(); }
/** * 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
if ((--callLevel == 0) && (command == STOP)) resetStop(); }
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){}
if ((--callLevel == 0) && (command == STOP)) resetStop(); }
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
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 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.<BR> * <B>NOTE:</B> 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. <BR> * 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