Hacking Roomba - Tod E.Kurt Part 5 potx

103 Chapter 5 — Driving RoombaF IGURE 5-7: Common preprogrammed Roomba moves Listing 5-4: The Main Loop of Waggle.java... Roomba Sensors Figure 6-1 shows the location of all the physica

Listing 5-3 Continued

pause( (int)(pausetime*1000) );

stop();

}public void spinLeft( int angle ) {if( angle < 0 ) return;

float pausetime = Math.abs(millimetersPerDegree *angle/speed);

spinLeftAt( Math.abs(speed) );

pause( (int)(pausetime*1000) );

stop();

}

Pivoting around a particular angle looks like this:

roombacomm.spinLeft(90); // turn anti-clockwise 90 degreesroombacomm.spinRight(360); // spin totally around

Moving in Curves

The straight motion and pivot turns are sufficient to move around, but arc turns are more cient when dealing with obstacles Watch the Roomba while it operates and you’ll notice itperforms three basic types of non-pivot turns (see Figure 5-7):

effi-䡲 Small circle: The Roomba moves in a circle with a radius a little bigger than itself This

is performed when the Roomba bumps against something (usually a table leg) and tries

to drive around it

䡲 Waggle: The Roomba moves forward by moving in alternating arcs with radii larger

than the Roomba This is performed when the Roomba is searching for something (dirt,home base, and so on)

䡲 Expanding spiral: The Roomba moves in a circle that gets bigger and bigger,

accom-plished by gradually increasing the radius over time This is performed when theRoomba thinks it is in a large open area of floor

The small circle is pretty evident now You can implement it in RoombaComm with thing like:

some-roombacomm.drive(200,300); // 250 mm/s, turn left on 300 mm radiusYou can test that out with the roombacomm.Driveprogram

The waggle is just the same, but with a larger radius, approximately 600, and it switches everyquarter second or so To emulate it, you could do something in a loop for however long youwant, like Listing 5-4

103 Chapter 5 — Driving Roomba

F IGURE 5-7: Common preprogrammed Roomba moves

Listing 5-4: The Main Loop of Waggle.java

You can test this out with the roombacomm.Waggletest program.

By far the most interesting curve the Roomba does is the expanding spiral, usually when it firststarts up You can implement a spiral by keeping a fixed velocity and incrementing the radiusvalue over time In the example program Spiral.java, you can see one way to go about this.Listing 5-5 shows the important parts of Spiral.java

Listing 5-5: Important Parts of Spiral.java

radius += radius_inc;

roombacomm.pause(pausetime);

}

An initial radiusis defined that is then incremented by radius_incamount every

pausetimemilliseconds Try modifying Spiral.java to choose different values for those threenumbers With the numbers in the listing, Roomba spirals out, but if you choose a large initial

radiusand a negative radius_inc, the Roomba would spiral inward

Real-Time Driving

The problem with the distance-based methods is the pause()method in them that causesyour program to wait (or block) Although it is paused, it can’t do anything else, such as checkfor user input or read the Roomba’s sensors A refinement you could add to RoombaCommwould be to implement a way to drive a certain distance without blocking the code One way

to do this might be to put all Roomba control code in one thread and sensor reading or interface checking in another thread

user-However, if you want to drive your Roomba in real time, you don’t necessarily need to drivespecific distances or angles By using the simplest methods like goForward(),spinLeft(),and spinRight(), coupled with stop(), you can create a program to drive the Roombafrom your computer

In Java the easiest way to respond to single key presses is to write a GUI program Java makes itquite easy to build GUI programs with buttons and windows and such The toolkit one gener-ally uses to make GUI Java programs is called Swing and is part of Java Normally these SwingGUI elements handle mouse clicks and key presses for you, but you can override that function-ality and have your own program listen to key presses This is done by making your code’s classimplement a KeyListenerand having a method called keyPressed()

105 Chapter 5 — Driving Roomba

Listing 5-6 shows the DriveRealTime program It is launched from the command line as

usual:

% /runit.sh roombacomm.DriveRealTime /dev/cu.KeySerial1

Figure 5-8 shows what it looks like after a few seconds of use

F IGURE 5-8: DriveRealTime program in use

DriveRealTime subclasses a JFrame(an application window in Java Swing) The setupWindow()

method holds all the things necessary to make the window work properly Most important is

addKeyListener(this), which will make the keyPressed()method of DriveRealTime

get called whenever you press a key The last thing setupWindow()does is show the window

with setVisible(true) To make the program a little more visually interesting, a JTextArea

is created and whenever you press a key, the text area is updated with the command you pressed

String portname = args[0];

roombacomm = new RoombaCommSerial();

Continued

Listing 5-6 Continued

if( !roombacomm.connect(portname) ) {System.out.println(“Couldn’t connect to “+portname);System.exit(1);

}roombacomm.startup();

roombacomm.control();

roombacomm.pause(50);

setupWindow();

}public void keyPressed(KeyEvent e) {int keyCode = e.getKeyCode();

if( keyCode == KeyEvent.VK_SPACE ) {updateDisplay(“stop”);

roombacomm.stop();

}else if( keyCode == KeyEvent.VK_UP ) {updateDisplay(“forward”);

roombacomm.goForward();

}else if( keyCode == KeyEvent.VK_DOWN ) {updateDisplay(“backward”);

roombacomm.goBackward();

}else if( keyCode == KeyEvent.VK_LEFT ) {updateDisplay(“spin left”);

roombacomm.spinLeft();

}else if( keyCode == KeyEvent.VK_RIGHT ) {updateDisplay(“spin right”);

roombacomm.spinRight();

}}public void updateDisplay( String s ) {displayText.append( s+”\n” );

}public void setupWindow() {displayText = new JTextArea(20,30);

content.add(scrollPane, BorderLayout.CENTER);

addKeyListener(this);

107 Chapter 5 — Driving Roomba

Writing Logo-Like Programs

In the 1980s a computer programming language called Logo became popular, targeted toward

kids or others with little computer experience It was famous for turtle graphics, a method of

drawing images on the screen by moving a virtual turtle around that had a pen strapped to it.

Although anyone who knew Logo knew about this scribbling turtle, Logo originally didn’t

have it when it was invented in the late 1960s The turtle idea came about afterward from a

real radio-controlled floor roaming robot named Irving that had touch sensors and could move

backward and forward and rotate left and right Sound familiar?

Logo programs could be very simple, like this one to draw a square:

When the program was run, it would output an image like Figure 5-9

F IGURE 5-9: Logo output for the square program

One could even define functions to encapsulate behavior The square above could be written as:

TO SQUAREREPEAT 4[FORWARD 100 LEFT 90]

ENDThese RoombaComm commands to go specific distances and rotate specific angles mirrorLogo’s commands You can create a Java version of the Logo square using RoombaCommlike so:public void square() {

for( int i=0; i<4; i++ ) {roombacomm.goForward(100);

roombacomm.spinLeft(90);

}}Not as simple as Logo perhaps but still compact and understandable And of course, like theoriginal turtle Irving, the Java code controls a real robot roaming around on the floor

Summary

Making the Roomba move around is the most important and most fun part of controlling it as

a robot From sending raw command bytes to high-level drive commands, you can now makeRoomba move along any conceivable path The ROI protocol’s abstraction away from com-manding each motor means you can easily move the Roomba in graceful circular curves, butwith the relevant equations you can subvert that and control each drive wheel independently.You can even control the Roomba in real time with the cursor keys on a keyboard and can nowmake any even your computer control the Roomba

The idea of encapsulating paths as functions in Logo is a powerful one, and you can start ing functions to draw all sorts of shapes with Roomba Understanding how to implement some

build-of the existing paths performed by Roomba is the basis for creating alternative moves for it thatwill work better in your environment

Reading the

Roomba Sensors

Arobot without sensors is merely a fancy machine Some robots use

humans as their sensors (and brain) If you’ve ever watched Battle

Bots or similar programs, you’ve seen what are really telepresence

robots and not robots in the purer sense of the term

Human senses have a huge range of fidelity The ear and eye can detect

information through a wide range of the electromagnetic spectrum The

sense of touch can detect heat and cold, variations in pressure, and damage

The vestibular system provides a high-resolution sense of balance and

ori-entation By comparison, a robot’s sensors are typically very simple They

detect a single, very specific kind of information: a touch sensor that

trig-gers a switch, an “eye” that detects only a single color of light and only one

pixel, or an “ear” that can only hear on frequency of sound

Currently, creating sensors that mimic the dynamic range of human senses

is expensive and computationally complex As we have learned from the

simpler life forms, sensors don’t need to be complex or high-fidelity to be

useful And as the subsumption architecture style of AI has shown, any

interaction with the environment is better than none

The Roomba has a wide variety of sensors for a robot that is so inexpensive

and so single purpose Roomba has touch sensors, rudimentary vision, and

an internal sense of orientation, just like the people it works for, but you’ll

find its version of those senses are both unique to it and much simplified

And unlike people, you’ll see it has a sense for dirt that could be best

described as licking the carpet

Roomba Sensors

Figure 6-1 shows the location of all the physical sensors present in Roomba

The entire front bumper is bristling with sensors It is the Roomba’s “head”

in more than one way In normal operation, the Roomba always moves

for-ward, so it can observe the world

 Send and receive sensor details

 Master the ROI SENSORS command

 Parse sensor data

 Make Roomba autonomous

 Measure distance and angles

 Spy on your Roomba

chapter

in this chapter

Turning over the Roomba, as in Figure 6-2, shows what some of these sensors look like.Actually, there’s not much to see There are no probes or whiskers or obvious sensors besidesthe fact that the bumper moves The two silver squares near the front caster are the chargingplates for the recharge dock With the brushes removed, you can see the two silver discs thatcompose the dirt sensor By just looking at the Roomba, it’s hard to tell exactly how it manages

to detect cliffs or walls

F IGURE 6-1: Location of Roomba sensors

Almost all of the physical sensors in the Roomba are implemented as an optical emitter/detector pair Even sensors that could be switches like the bumpers are implemented optically.Optical sensors have the advantage of being high-wear due to less physical interaction Theyare usually more complex (and thus expensive) to implement

Figure 6-3 shows three of the most common examples of emitter/detector configurations

status

dirt detect

power clean spot max

Cliff Left Bump Left

Cliff Front Left

Cliff Right Bump Right Cliff Front Right

Remote Control, Virtual Wall

Wall

Wheeldrop and Motor Overcurrent

Buttons Dirt Detect

111 Chapter 6 — Reading the Roomba Sensors

F IGURE 6-2: Underside of Roomba, showing sensors

F IGURE 6-3: Common optical emitter/detector configurations

Optoisolator: Remote Controls, Virtual Wall, and Dock

The emitter in an optoisolator is usually an infrared LED and the detector is a matchinginfrared phototransistor or photodiode The first type shown in Figure 6-3 is an optoisolator.The light from the LED travels unobstructed to the detector Optoisolators are used to separate circuits that would be too dangerous or too physically far apart to connect electrically.Common optoisolator uses are motor driver circuits (to separate high-power circuits from low-power circuits) and remote controls for TVs and stereos You could also consider the high-speed fiber optic links that drive the Internet as a type of optoisolator In the case of Roomba,the clear circular bump on the front top is the detector half of an optoisolator It receivesremote control commands and “sees” the virtual wall and docking base

Optical Interrupter: Wheel Speed and Bump Detection

The optical interrupter configuration is the next type shown in Figure 6-3 It adds a physicalbarrier that is inserted between the emitter and detector to indicate some event The Roomba’smotors have a toothed disc (shown in Figure 5-2) that rotates between an emitter/detector pair.Each pulse of light received by the detector indicates some number of degrees of rotation Forthe Roomba’s bump sensors, a plastic tab is attached to the bumper and when the bumper ispushed in, the tab interrupts the beam of light between the LED and the detector Figure 6-4shows what the bump sensor actually looks like It’s the black plastic box attached directly tothe main green circuit board (oriented vertically with wires attaching to it in Figure 6-4) Theemitter and detector are oriented vertically, with a horizontal black plastic tab as the inter-rupter The black tab is part of the whole black bumper and moves when it does You can justsee the spring on the left (the shiny coil of steel) that pushes the bumper back out

F IGURE 6-4: Bump sensor, an optical interrupter

113 Chapter 6 — Reading the Roomba Sensors

Optical Object Detector: Cliff and Wall Detection

The third configuration rotates the emitter and detector so they don’t directly see each other

and instead point toward something that may be there If it is there, the LED’s light is reflected

by it and the detector sees it This is how the Roomba’s cliff and wall sensors are implemented

Figure 6-5 shows the wall sensor on the right side of the Roomba It is two wells in the bumper:

the long deep one on the right houses the LED and the small one on the left houses the

photodetector

F IGURE 6-5: Wall sensors, an optical object detector

Although the optical sensors all use infrared light, some digital cameras can see that light Turn your

Roomba over when it’s on and point your camera at it Through the electronic viewfinder you

should see the glow of LEDs

Micro-Switches: Wheeldrop and Buttons

The wheeldrop and button sensors are implemented with standard micro-switches They get

much less use, so a mechanical switch was deemed sufficient by iRobot engineers Figure 6-6

shows what one of these micro-switches looks like This particular one is the wheeldrop sensor

for the left wheel The switch is the small rectanglar box glued to a screwed-down mount It

has a black tip with two light-colored wires coming from it The tip is what moves to trigger the

switch The wheel unit pushes against it when the wheel is up, and stops pushing it when the

wheel drops The black disc behind the wheeldrop switch with dark wires coming from it is

the Roomba’s speaker

Other Sensor Implementations

Figure 6-7 shows the internals of the Roomba The two large silver boxes located right belowthe vacuum brush motor are the dirt sensors Inside each of those magic boxes is a dense analogcircuit that implements something similar to those capacitive touch sensors on table lamps.The motor over-current sensor is presumably determined by a current sense somewhere in themotor driver circuit

F IGURE 6-6: Wheeldrop sensor, a microswitch

F IGURE 6-7: Roomba internals

115 Chapter 6 — Reading the Roomba Sensors

ROI SENSORS Command

The ROI presents the sensor data in a lightly processed form available through the SENSORS

command This command can retrieve subsets of the entire sensor payload, but for all the code

presented here, you will fetch the entire set

Sending the SENSORS Command

Sending the SENSORScommand is easy; it’s the receiving of the data and parsing it that can

be problematic To send the SENSORScommand that retrieves all of the sensor data using

RoombaComm, any of the following are valid:

// using send() and literal bytes in an array

The last one is what’s normally used When this command is sent, Roomba responds by

send-ing the sensor packet in usually less than 100 milliseconds The actual time it takes depends on

the serial interface used (Bluetooth adapters have a higher latency at times) and other factors

that aren’t entirely clear

Receiving the Sensor Data

The SENSORScommand is the only one that returns data All the other commands are

one-way Receiving data can be tricky because the only way to know if you’ve read all the data is to

wait for all the bytes to come back An initial implementation of getting sensor data might be

(in pseudo-code):

send(SENSORS, SENSORS_ALL);

byte data[] = receive(26); // waits for 26 bytes of data

Unfortunately if this hypothetical receive()function never gets all 26 bytes, then it either

waits forever or times out Even if it works every time, every time you get data, you block the

rest of your program until all the data is obtained

A common way to deal with this problem in Java is to spawn another thread and tell it to go

deal with the waiting around It performs its task and then notifies the main code, returning

the result This callback or event pattern is used a lot when dealing with the real world In Java,

you’ll use the RXTX library’s ability to call back to you when the Roomba sends data

Receiving Serial Events

To get serial events, you first have to tell the RXTX SerialPortobject that you want to listen

to events The open_port()method in RoombaCommSerialshows how this is done Theimportant parts are:

port = (SerialPort)portId.open(“roomba serial”, 2000);

port.addEventListener(this);

port.notifyOnDataAvailable(true);

That code snippet gets the serial port object, adds RoombaCommSerialas an event listener,and then specifies the kind of event you care about You’ll find several types of events Themost useful for RoombaComm (and the most reliably implemented in RXTX) is the event thattells you when serial input data is available

The serialEvent()method that will be called when data is available looks like Listing 6-1

Listing 6-1: RoombaCommSerial.serialEvent()

byte[] sensor_bytes = new byte[26];

byte buffer[] = new byte[26];

boolean sensorsValid = false;

int bufferLast;

synchronized public void serialEvent(SerialPortEventserialEvent) {

try {if( serialEvent.getEventType() ==

SerialPortEvent.DATA_AVAILABLE ) {while( input.available()>0 ) {buffer[bufferLast++] = (byte)input.read();

if( bufferLast==26 ) {bufferLast = 0;

System.arraycopy(buffer,0,sensor_bytes,0,26);

sensorsValid = true;

computeSensors();

}} // while}

} catch (IOException e) {errorMessage(“serialEvent”, e);

}}

This is one of the most complex bits of code in the entire book, and it really isn’t that bad.The first two lines are variables inside of the RoombaCommSerialobject that create two bytebuffers The sensor_bytes buffer holds known good sensor data from the last sensor read,and sensorsValidtells you if sensor_bytesis good or not The other buffer, just called