Hacking Roomba - Tod E.Kurt Part 5 potx

102 Part I — Interfacing 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 degrees roombacomm.spinRight(360); // spin totally around Moving in Curves The straight motion and pivot turns are sufficient to move around, but arc turns are more effi- cient when dealing with obstacles. Watch the Roomba while it operates and you’ll notice it performs three basic types of non-pivot turns (see Figure 5-7): Ⅲ 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 the Roomba thinks it is in a large open area of floor. The small circle is pretty evident now. You can implement it in RoombaComm with some- thing like: roombacomm.drive(200,300); // 250 mm/s, turn left on 300 mm radius You can test that out with the roombacomm.Drive program. The waggle is just the same, but with a larger radius, approximately 600, and it switches every quarter second or so. To emulate it, you could do something in a loop for however long you want, 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 while( !done ) { roombacomm.drive(200,600); roombacomm.pause(250); roombacomm.drive(200,-600); roombacomm.pause(250); } status dirt detect power max clean spot status dirt detect power max clean spot Small circle Expanding spiral Waggle status dirt detect power max clean spot 104 Part I — Interfacing You can test this out with the roombacomm.Waggle test program. By far the most interesting curve the Roomba does is the expanding spiral, usually when it first starts up. You can implement a spiral by keeping a fixed velocity and incrementing the radius value 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 int radius = 10; int radius_inc = 20; int pausetime = 500; int speed = 250; while( !done ) { roombacomm.drive(speed,radius); radius += radius_inc; roombacomm.pause(pausetime); } An initial radius is defined that is then incremented by radius_inc amount every pausetime milliseconds. Try modifying Spiral.java to choose different values for those three numbers. With the numbers in the listing, Roomba spirals out, but if you choose a large initial radius and 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 causes your program to wait (or block). Although it is paused, it can’t do anything else, such as check for user input or read the Roomba’s sensors. A refinement you could add to RoombaComm would 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 user- interface checking in another thread. However, if you want to drive your Roomba in real time, you don’t necessarily need to drive specific distances or angles. By using the simplest methods like goForward(), spinLeft(), and spinRight(), coupled with stop(), you can create a program to drive the Roomba from your computer. In Java the easiest way to respond to single key presses is to write a GUI program. Java makes it quite 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 Swing GUI 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 class implement a KeyListener and 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 through updateDisplay(). Listing 5-6: DriveRealTime.java public class DriveRealTime extends JFrame implements KeyListener { RoombaCommSerial roombacomm; JTextArea displayText; public static void main(String[] args) { new DriveRealTime(args); } public DriveRealTime(String[] args) { super(“DriveRealTime”); String portname = args[0]; roombacomm = new RoombaCommSerial(); Continued 106 Part I — Interfacing 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); displayText.setLineWrap(true); displayText.setEditable(false); displayText.addKeyListener(this); JScrollPane scrollPane = new JScrollPane(displayText, JScrollPane.VERTICAL_SCROLLBAR_ALWAYS, JScrollPane.HORIZONTAL_SCROLLBAR_NEVER); Container content = getContentPane(); content.add(scrollPane, BorderLayout.CENTER); addKeyListener(this); 107 Chapter 5 — Driving Roomba Listing 5-6 Continued pack(); setResizable(false); setVisible(true); } } 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: FORWARD 100 LEFT 90 FORWARD 100 LEFT 90 FORWARD 100 LEFT 90 FORWARD 100 LEFT 90 When the program was run, it would output an image like Figure 5-9. F IGURE 5-9: Logo output for the square program 108 Part I — Interfacing One could even define functions to encapsulate behavior. The square above could be written as: TO SQUARE REPEAT 4[FORWARD 100 LEFT 90] END These RoombaComm commands to go specific distances and rotate specific angles mirror Logo’s commands. You can create a Java version of the Logo square using RoombaComm like 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 the original 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 make Roomba 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, but with 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 now make any even your computer control the Roomba. The idea of encapsulating paths as functions in Logo is a powerful one, and you can start build- ing functions to draw all sorts of shapes with Roomba. Understanding how to implement some of the existing paths performed by Roomba is the basis for creating alternative moves for it that will work better in your environment. Reading the Roomba Sensors A robot 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 110 Part I — Interfacing 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 besides the fact that the bumper moves. The two silver squares near the front caster are the charging plates for the recharge dock. With the brushes removed, you can see the two silver discs that compose 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. They are 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 max clean spot 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 Optical Interrupter Optical Object Detector Emitter LED Detector Phototransistor or Photodiode Object (Wall, Floor, etc.) [...]... 1148 355 9178 65: bump:_r 1148 355 918417:bump:lr 1148 355 918969:bump: 1148 355 91 952 9:bump: 1148 355 920089:bump:l_ 1148 355 920649:bump: 1148 355 921202:bump:_r 1148 355 921 754 :bump:_r 1148 355 922306:bump:_r 1148 355 922907:bump:_r 1148 355 923 459 :bump:_r 1148 355 924011:bump:_r wheel: _ wheel: _ wheel: _ wheel: _ wheel:_c_ wheel: _ wheel:_c_ wheel:_c_ wheel:lcr wheel:lcr wheel:lcr wheel:lcr Chapter 6 — Reading the Roomba. .. “/dev/cu.KeySerial1”; RoombaCommSerial roombacomm = new RoombaCommSerial(); if( ! roombacomm.connect(portname) ) { System.out.println(“Couldn’t connect to “+portname); System.exit(1); } roombacomm.startup(); roombacomm.control(); roombacomm.pause(100); roombacomm.updateSensors(); boolean done = false; while( !done ) { if( roombacomm.bumpLeft() ) { roombacomm.spinRight(90); } else if( roombacomm.bumpRight() ) { roombacomm.spinLeft(90);... using RoombaComm, any of the following are valid: // using send() and literal bytes in an array byte cmd = {(byte)0x8e,(byte)0x00}; roombacomm.send(cmd); // using two sends and RoombaComm defines roombacomm.send((byte)RoombaComm.SENSORS); roombacomm.send((byte)RoombaComm.SENSORS_ALL); // using high-level command roombacomm.sensors(RoombaComm.SENSORS_ALL); // using high-level command default roombacomm.sensors();... loop varies between 1 050 milliseconds and 2000 milliseconds, depending on whether or not the Roomba is plugged in At the expense of having little complexity, you can have an optimally responsive loop that is able to do both things not related to Roomba and Roomba sensor-related things as soon as the sensor data is available Listing 6 -5 shows an example of such a loop Listing 6 -5 : Non-blocking Periodic... sensors later in this book 127 Fun Things to Do part in this part Chapter 7 Making RoombaView Chapter 8 Making Roomba Sing Chapter 9 Creating Art with Roomba Chapter 10 Using Roomba as an Input Device Making RoombaView chapter W ith a complete Roomba API like RoombaComm, it’s now possible to build an application on your computer that is both a control panel for Roomba with many more features than the standard... can now create a program to spy on Roomba as it goes about its business Listing 6-7 shows the main() method of a simple Spy.java program Listing 6-7 : The main() Method of Spy.java static int pausetime = 50 0; public static void main(String[] args) { // standard RoombaComm setup boolean done = false; while( !done ) { roombacomm.updateSensors(); printSensors(roombacomm); roombacomm.pause(pausetime); done... little 26-byte array, the task is to figure out how to parse it Recall from Figures 2 -5 , 2-6 , and 2-7 the organization of the sensor data packet For SENSORS_ALL, Roomba sends the data as a single 26-byte chunk The first byte is the bumps and wheeldrops bit field, the second byte is the wall value, and so on The data has different formats: Ⅲ Word values: Two bytes joined together form a 16-bit value... m ax de dir te t ct B Start Angle= – 45 Distance= 150 FIGURE 6-8 : Sensor reading for distance and angle In case A, odometry data is read at an interval that captures an accurate view of the Roomba s motion In case B the data is read at a much slower interval, and the result is a distorted view of where the Roomba really is 1 25 126 Part I — Interfacing Spying on Roomba With all these new sensor tools... 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 FIGURE 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... sensors are valid This non-blocking will be used in Chapter 7 as part of the RoombaView application BumpTurn: Making an Autonomous Roomba Armed with the techniques from above, you can now start creating some programs that make an autonomous Roomba By reading sensors, you can make Roomba know about its environment, and by commanding its motors, you can make it move around Listing 6-6 shows the entirety . is inserted between the emitter and detector to indicate some event. The Roomba s motors have a toothed disc (shown in Figure 5- 2 ) that rotates between an emitter/detector pair. Each pulse of. 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. 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