added stuff

1 files changed, 1690 insertions, 0 deletions

diff --git a/roombacomm-client/src/com/hackingroomba/roombacomm/RoombaComm.java b/roombacomm-client/src/com/hackingroomba/roombacomm/RoombaComm.java
new file mode 100644
index 0000000..733ae53
--- /dev/null
+++ b/roombacomm-client/src/com/hackingroomba/roombacomm/RoombaComm.java
@@ -0,0 +1,1690 @@
+/*
+ *  RoombaComm Interface
+ *
+ *  Copyright (c) 2006 Tod E. Kurt, tod@todbot.com, ThingM
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General
+ *  Public License along with this library; if not, write to the
+ *  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
+ *  Boston, MA  02111-1307  USA
+ *
+ */
+
+
+package com.hackingroomba.roombacomm;
+
+import gnu.io.SerialPort;
+
+import java.io.BufferedReader;
+import java.io.BufferedWriter;
+import java.io.FileReader;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.io.InputStreamReader;
+import java.util.Map;
+
+/**
+ * The abstract base for all Roomba communications.
+ * 
+ * <h2> Overview </h2>
+ * This class contains the communications layer-independent parts of
+ * how to communicate with a Roomba.  It does assume a very serial port-like
+ * interaction.
+ *
+ * Standard lifecyle of this object (and its subclasses) <pre>
+ *   RoombaComm roomba = new RoombaCommSubClass();  // (e.g. RoombaCommSerial)
+ *   roomba.listports();               // if implemented
+ *   roomba.connect("someportid");
+ *   roomba.startup();
+ *   roomba.updateSensors();
+ *   while( ... ) {
+ *      roomba.sensors();
+ *      roomba.playNote( 53, 12 );
+ *      roomba.goForward( 400 );
+ *      roomba.spinRight( 45 );
+ *      if( roomba.bump() ) roomba.goBackward( 100 );
+ *   }    
+ *   roomba.disconnect();
+ * </pre>
+ *
+ * <h2> API levels </h2>
+ * Describe different API levels
+ *
+ * <h2> Sensor Functions </h2>
+ * Describe sensor functions
+ *
+ * <h2> Sublass behavior </h2>
+ * Describe subclassing strategries
+ *
+ *
+ * @author Tod E. Kurt
+ * SVN id value is $Id: RoombaComm.java 182 2010-11-02 03:49:10Z bouchier $
+ */
+public abstract class RoombaComm
+{
+    /** version of the library */
+    static public final String VERSION = "0.96.3";
+
+	/**
+	 * contains a list of all the ports
+	 * keys are port names (e.g. "/dev/usbserial1")
+	 * values are Boolean in-use indicator
+	 */
+	protected static Map ports = null;
+	
+    /** turns on/off various debugging messages */
+    public boolean debug = false;
+    
+    public boolean isDebug() {
+		return debug;
+	}
+
+	public void setDebug(boolean debug) {
+		this.debug = debug;
+	}
+
+	/** distance between wheels on the roomba, in millimeters */
+    public static final int wheelbase = 258; 
+    /** mm/deg is circumference distance divided by 360 degrees */
+    public static final float 
+        millimetersPerDegree = (float)(wheelbase * Math.PI / 360.0);
+    /** mm/rad is a circumference distance divied by two pi */
+    public static final float 
+        millimetersPerRadian = (float)(wheelbase/2);
+
+    /** default speed for movement operations if speed isn't specified */
+    public static final int defaultSpeed  =  200;
+
+    /** default update time in ms for auto sensors update */
+    public static final int defaultSensorsUpdateTime = 200;
+       
+    /** current mode, if known */
+    int mode;
+
+    /** current speed for movement operations that don't take a speed */
+    public int speed = defaultSpeed;
+
+    /** computed boolean for when Roomba is errored out of safe mode */
+    boolean safetyFault = false;
+    /** if sensor variables have been updated successfully */
+    protected boolean sensorsValid = false;
+    /** Set to true to make sensors auto-update (at expense of serial b/w) */
+    boolean sensorsAutoUpdate = false;
+    /** Time in milliseconds between sensor updates  */
+    int sensorsUpdateTime = 200;
+    /** last time (System.currentTimeMillis) that the sensors were updated */
+    protected long sensorsLastUpdateTime;
+    /** how many bytes we expect to read from the sensor command */
+    protected int readRequestLength;
+
+    /** internal storage for all roomba sensor data */
+    protected byte[] sensor_bytes = new byte[1024];
+  
+    /** connected to a serial port or not, not necessarily to roomba */
+    boolean connected = false;
+
+    public boolean isConnected() {
+    	String str = this.getSensorsAsString();
+    	if (str != null && str.length() >=1){
+    		if (debug){
+    			logmsg("isConnected found sensorString as ("+str+")");
+    		}
+    		this.setConnected(true);
+    		return true;
+    	}else{
+    		if (debug){
+    			if (str != null){
+    				logmsg("isConnected found sensorString ("+str+")");
+    			}else{
+    				logmsg("isConnected found sensorString (null)");
+    			}
+    		}
+    	}
+		return connected;
+	}
+
+	public void setConnected(boolean connected) {
+		this.connected = connected;
+	}
+
+	/** set of flgs for the current state of the LEDs */
+    /** note this is a superset of all protocol's supported */
+    private boolean redOn = false;
+    private boolean greenOn = false;
+    private boolean toggleSpot = false;
+    private boolean toggleClean = false;
+    private boolean toggleMax = false;
+    private boolean toggleDirt = false;
+    private boolean toggleDock = false;
+    private boolean toggleCheckRobot = false;
+    private int power_color = 0;
+    private int power_int = 0;
+	
+	/** default RoombaComm protocol to identify classes of API calls to be made */
+	private String protocol = "SCI";
+	/** default baud rate for the default protocol */
+	protected int rate = 57600;
+	
+	protected String portname = null;
+	/** connection object to use when appropriate */
+    RobotConnection robotConnection;
+
+
+	/** 
+	 * Some "virtual" serial ports like Bluetooth serial on Windows
+	 * return weird errors deep inside RXTX if an opened port is used
+	 * before the virtual COM port is ready.  One way to check that it 
+	 * is ready is to look for the DSR line going high.  
+	 * However, most simple, real serial ports do not do hardware handshaking
+	 * so never set DSR high.
+	 * Thus, if using Bluetooth serial on Windows, do:
+	 *  roombacomm.waitForDSR = true;
+	 * before using it and see if it works.
+	 */
+	public boolean waitForDSR = false;
+
+	/** The RXTX port object, normally you don't need access to this */
+	public SerialPort serialPort = null;
+
+	public RoombaComm() {
+        connected = false;
+        mode = MODE_UNKNOWN;
+    }
+
+    public RoombaComm(boolean autoUpdate) {
+        this();
+        if( autoUpdate )
+            startAutoUpdate();
+    }
+
+    public RoombaComm(boolean autoUpdate, int updateTime) {
+        this(autoUpdate);
+        sensorsUpdateTime = updateTime;
+    }
+    
+    public RoombaComm(RobotConnection rc) {
+    	robotConnection = rc;
+    }
+    public void startAutoUpdate() {
+        new Thread( new Runnable() {
+                public void run() {
+                    try { 
+                        while( sensorsUpdateTime > 0 ) {
+                            if( connected() ) sensors();
+                            Thread.sleep( sensorsUpdateTime );
+                        }
+                    } catch(InterruptedException ex) {}
+                }
+            }).start();
+    }
+
+    /**
+     * List available ports
+     * @return a list available portids, if applicable
+     * or empty set if no ports, 
+     * or return null if list is not enumerable
+     */
+    public abstract String[] listPorts();
+
+    /**
+     * Connect to a port
+     * (for serial, portid is serial port name, for net, portid is url?)
+     * @return true on successful connect, false otherwise
+     */
+    public abstract  boolean connect(String portid);
+    /**
+     * Disconnect from a port, clean up any memory in use
+     */
+    public abstract  void disconnect();
+
+
+    /**
+     * Send given byte array to Roomba. 
+     * @param bytes byte array of ROI commands to send
+     * @return true on successful send
+     */
+    public abstract boolean send(byte[] bytes);
+
+    /** 
+     * Send a single byte to the Roomba 
+     * (defined as int because of stupid java signed bytes)
+     * @param b byte of an ROI command to send
+     * @return true on successful send
+     */
+    public abstract boolean send(int b);
+
+    /**
+     * Query Roomba for sensor status and sync its state with this object's
+     * Subclasses should query Roomba and fill up 'sensor_bytes' with the full
+     * sensor data set
+     * If a RooombaComm object is constructed with 'autoUpdate' true, 
+     * calling this method is not required because a separate thread is created
+     * to do sensor updating.
+     *
+     * @return true on successful sensor update, false otherwise
+     */
+    //public abstract boolean updateSensors();
+
+    /**
+     * Wake's Roomba up, if possible, thus optional
+     * To wake up the Roomba requires twiddling its DD line, often
+     * hooked up to the RS-232 DTR line, which may not be available in some 
+     * implementations
+     */
+    public void wakeup() {
+        logmsg("subclass has not implemented");
+//        byte cmd[] = { (byte)POWER, (byte)v, (byte)power_color, (byte)power_intensity };
+//        send(cmd);
+//        MSComm1.Output = "+++" & Chr(13)
+//        MSComm1.Output = "ATSW22,6,1,1" & Chr(13)
+//        MSComm1.Output = "ATSW23,6,0,1" & Chr(13)
+//        MSComm1.Output = "ATSW23,6,1,1" & Chr(13)
+//        MSComm1.Output = "ATMD" & Chr(13)
+        String str="+++\nATSW22,6,1,1\n,ATSW23,6,0,1\nATSW23,6,1,1\nATMD\n";
+        send(str.getBytes());
+//        byte bytes[] = str.getBytes();
+//        for (int i = 0; i < bytes.length; i++) {
+//        	
+//		}
+    }
+
+    /**
+     * Put Roomba in safe mode.
+     * As opposed to full mode.  Safe mode is the preferred working state
+     * when playing with the Roomba as it provides some measure of 
+     * autonomous self-preservation if it encounters a cliff or is picked up
+     * If that happens it goes into passive mode and must be 'reset()'.
+     * @see #reset()
+     */
+    public void startup() {
+        logmsg("startup");
+        speed = defaultSpeed;
+        start();
+    }
+
+    /** 
+     * Reset Roomba after a fault.  This takes it out of whatever mode it was
+     * in and puts it into safe mode.
+     * This command also syncs the object's sensor state with the Roomba's
+     * by calling updateSensors()
+     * @see #startup()
+     * @see #updateSensors()
+     */
+    public void reset() {
+        logmsg("reset");
+        stop();
+        startup();
+        control();
+        updateSensors();
+    }
+
+    /**  Send START command  */
+    public void start() { 
+        logmsg("start");
+        mode = MODE_PASSIVE;
+        send( START );
+    }
+    /**  Send CONTROL command  */
+    public void control() { 
+        logmsg("control");
+        mode = MODE_SAFE;
+        send( CONTROL );
+        // set blue dirt LED on so we know roomba is powered on & under control
+        // (and we don't forget to turn it off, and run it's batteries flat)
+        // FIXME: first time after a poweron, the lights flash then turn off
+        setLEDs(false, false, false, false, false, true, 128, 255);
+    }
+    /**  Send SAFE command  */
+    public void safe() { 
+        logmsg("safe");
+        mode = MODE_SAFE;
+        send( SAFE );
+    }
+    /**  Send FULL command  */
+    public void full() { 
+        logmsg("full");
+        mode = MODE_FULL;
+        send( FULL );
+    }
+
+    /**
+     * Power off the Roomba.  Once powered off, the only way to wake it
+     * is via wakeup() (if implemented) or via a physically pressing
+     * the Power button
+     * @see #wakeup()
+     */
+    public void powerOff() {
+        logmsg("powerOff");
+        mode = MODE_UNKNOWN;
+        send( POWER );
+    }
+
+    /** Send the SPOT command */
+    public void spot() {
+        logmsg("spot");
+        mode = MODE_PASSIVE;
+        send( SPOT );
+    }
+    /** Send the CLEAN command */
+    public void clean() {
+        logmsg("clean");
+        mode = MODE_PASSIVE;
+        send( CLEAN );
+    }
+    /** Send the max command */
+    public void max() {
+        logmsg("max");
+        mode = MODE_PASSIVE;
+        send( MAX );
+    }
+    /** Send the max command */
+    public void dock() {
+        logmsg("dock");
+        mode = MODE_PASSIVE;
+//        send( CLEAN );
+        send( DOCK );
+    }
+    /** 
+     * Send the SENSORS command 
+     * with one of the SENSORS_ arguments
+     * Typically, one does "sensors(SENSORS_ALL)" to get all sensor data
+     * @param packetcode one of SENSORS_ALL, SENSORS_PHYSICAL, 
+     *                   SENSORS_INTERNAL, or SENSORS_POWER, or for roomba 5xx, it
+     *                   is the sensor packet number (from the spec)
+     */
+    public void sensors(int packetcode ) {
+    	sensorsValid = false;
+        logmsg("sensors:"+packetcode);
+        switch (packetcode) {
+        case 0: readRequestLength = 26; break;
+        case 1: readRequestLength = 10; break;
+        case 2: readRequestLength = 6; break;
+        case 3: readRequestLength = 10; break;
+        case 4: readRequestLength = 14; break;
+        case 5: readRequestLength = 12; break;
+        case 6: readRequestLength = 52; break;
+        case 100: readRequestLength = 80; break;
+        case 101: readRequestLength = 28; break;
+        case 106: readRequestLength = 12; break;
+        case 107: readRequestLength = 9; break;
+        case 19:
+        case 20:
+        case 22:
+        case 23:
+        case 25:
+        case 26:
+        case 27:
+        case 28:
+        case 29:
+        case 30:
+        case 39:
+        case 40:
+        case 41:
+        case 42:
+        case 43:
+        case 44:
+        case 46:
+        case 47:
+        case 48:
+        case 49:
+        case 50:
+        case 51:
+        case 54:
+        case 55:
+        case 56:
+        case 57: readRequestLength = 2; break;
+        default: readRequestLength = 1; break;
+        }
+        
+        byte cmd[] = { (byte)SENSORS, (byte)packetcode};
+        send(cmd);
+    }
+    
+    /** 
+     * get all sensor data
+     */
+    public void sensors() {
+    	readRequestLength = 26;
+        sensors( SENSORS_ALL );
+    }
+	/**
+	 * Read roomba 26-byte sensor record using robotConnection. Tries once to read valid data, allowing 100ms
+	 * timeout on each attempt. 
+	 * @return true if read 26 bytes of valid data. Data has been stored in sensor_bytes. False otherwise
+	 */
+ 	public boolean updateSensors()
+ 	{
+ 		return updateSensors(SENSORS_ALL);
+ 	}
+ 	
+ 	public boolean updateSensors(int sensorGroup)
+ 	{
+    	int sensorGroupSize;
+    	
+ 		if (robotConnection == null) {
+ 	 		System.out.println("Error at ArduinoBot.updateSensors(): no connection object for robot");
+ 	 		return false; 			
+ 		}
+
+ 		switch(sensorGroup) {
+ 		case SENSORS_ALL: sensorGroupSize = 26; break;
+ 		case 100: sensorGroupSize = 80; break;
+ 		default:
+ 			System.err.println("Invalid sensor group in updateSensors(): " + sensorGroup);
+ 			return false;
+ 		}
+    	sensors(sensorGroup);
+ 		return getSensorData(sensorGroupSize); 		
+ 	}
+
+    /**
+     * Query a list of sensors. This is a roomba 5xx only command.
+     * @param sensorList A byte array containing the sensor groups requested to be read
+     * @param returnLen The number of bytes of data expected to be returned from roomba
+     */
+    public void queryList(byte[] sensorList, int returnLen)
+    {
+    	int i = 0;
+    	int j;
+    	
+    	sensorsValid = false;
+    	readRequestLength = returnLen;
+    	byte cmd[] = new byte[2+sensorList.length];
+    	cmd[i++] = (byte) QUERYLIST;
+    	cmd[i++] = (byte)sensorList.length;
+    	for (j=0; j<sensorList.length; j++)
+    		cmd[i++] = sensorList[j];
+    	send(cmd);
+    }
+
+	/**
+	 * @param sensorGroupSize
+	 */
+	public boolean getSensorData(int sensorGroupSize) {
+ 		byte [] readData;
+ 		
+		// try once to read valid sensor data before giving up
+		//startTime = System.currentTimeMillis();
+		try {
+			readData = robotConnection.readBot(sensorGroupSize);
+		} catch (Exception e) {
+			e.printStackTrace();
+			return false;
+		}
+
+		// readBot either read requested # of bytes or returned a null (indicating timeout. 
+		// If null or invalid data and group 0 (26 bytes expected), try again
+		if( readData != null ) { 
+			if (((readData[1] > 1) || (readData[1] < 0)) && (sensorGroupSize == 26)) {
+				sensorsValid = false;
+				logmsg("updateSensors: received invalid data while attempting to read Roomba sensors!");
+			} else {
+				sensorsValid = true;
+				System.arraycopy(readData, 0, sensor_bytes, 0, sensorGroupSize);
+				logmsg("updateSensors: sensorsValid!");            		
+				//elapsedTime = System.currentTimeMillis() - startTime;
+				return true;
+			}
+        }
+		System.out.println("Error: timeout on sensor read");
+		return false;
+	}
+    //
+    // basic functions
+    //
+
+    /** 
+     * Alias to pause
+     * @see #pause(int)
+     */
+    public void delay( int millis ) {  pause( millis );  }
+
+    /** 
+     * Just a simple pause function. 
+     * Makes the thread block with Thread.sleep()
+     * @param millis number of milliseconds to wait
+     */
+    public void pause( int millis ) {
+        try { Thread.sleep(millis); } catch(Exception e) { }
+    }
+
+
+    //
+    // higher-level functions
+    //
+
+    /**
+     * Stop Rooomba's motion.
+     * Sends drive(0,0)
+     */
+    public void stop() {
+        logmsg("stop");
+        drive( 0, 0 );
+    }
+
+    /** Set speed for movement commands */
+    public void setSpeed( int s ) { speed = Math.abs(s); }
+    /** Get speed for movement commands */
+    public int  getSpeed() { return speed; }
+
+    /**
+     * Go straight at the current speed for a specified distance.
+     * Positive distance moves forward, negative distance moves backward.
+     * This method blocks until the action is finished.
+     * @param distance distance in millimeters, positive or negative
+     */
+    public void goStraight( int distance ) {
+        float pausetime = Math.abs(distance / speed);  // mm/(mm/sec) = sec
+        if (distance > 0)
+        	goStraightAt( speed );
+        else
+        	goStraightAt( -speed);
+        pause( (int)(pausetime*1000) );
+        stop();
+    }
+
+    /**
+     * @param distance distance in millimeters, positive 
+     */
+    public void goForward( int distance ) {
+        if( distance < 0 ) return;
+        goStraight( distance );
+    }
+
+    /**
+     * @param distance distance in millimeters, positive 
+     */
+    public void goBackward( int distance ) {
+        if( distance < 0 ) return;
+        goStraight( -distance );
+    }
+
+    /**
+     *
+     */
+    public void turnLeft() {
+        turn(129);
+    }
+    public void turnRight() {
+        turn(-129);
+    }
+    public void turn( int radius ) {
+        drive( speed, radius );
+    }
+
+    /**
+     * Spin right or spin left a particular number of degrees
+     * @param angle angle in degrees, 
+     *              positive to spin left, negative to spin right
+     */
+    public void spin( int angle ) {
+        if( angle > 0 )       spinLeft( angle );
+        else if( angle < 0 )  spinRight( -angle );
+    }
+
+    /**
+     * Spin right the current speed for a specified angle 
+     * @param angle angle in degrees, positive
+     */
+    public void spinRight( int angle ) {
+        if( angle < 0 ) return;
+        float pausetime = Math.abs( millimetersPerDegree * angle / speed );
+        spinRightAt( Math.abs(speed) );
+        pause( (int)(pausetime*1000) );
+        stop();
+    }
+
+    /**
+     * Spin left a specified angle at a specified speed
+     * @param angle angle in degrees, positive
+     */
+    public void spinLeft( int angle ) {
+        if( angle<0 ) return;
+        //float pausetime = 
+        float pausetime = Math.abs( millimetersPerDegree * angle / speed );
+        spinLeftAt( Math.abs(speed) );
+        pause( (int)(pausetime*1000) );
+        stop();
+    }
+
+    /** 
+     * Spin in place anti-clockwise, at the current speed
+     */
+    public void spinLeft() {
+        spinLeftAt( speed );  
+    }
+    /** 
+     * Spin in place clockwise, at the current speed
+     */
+    public void spinRight() {
+        spinRightAt( speed );  
+    }
+
+    /**
+     * Spin in place anti-clockwise, at the current speed.
+     * @param aspeed speed to spin at
+     */
+    public void spinLeftAt(int aspeed) {
+        drive( aspeed, 1 ); 
+    }
+
+    /**
+     * Spin in place clockwise, at the current speed.
+     * @param aspeed speed to spin at, positive
+     */
+    public void spinRightAt(int aspeed) {
+        drive( aspeed, -1 ); 
+    }
+
+    //
+    // mid-level movement, no blocking, parameterized by speed, not distance
+    //
+
+    /** 
+     * Go straight at a specified speed.  
+     * Positive is forward, negative is backward
+     * @param velocity velocity of motion in mm/sec
+     */
+    public void goStraightAt( int velocity ) {
+        //System.out.println("goStraightAt: velocity:"+velocity);
+        if( velocity > 500 ) velocity = 500;
+        if( velocity < -500 ) velocity = -500;
+        drive( velocity, 0x8000 );
+    }
+
+    /**
+     * Go forward the current (positive) speed
+     */
+    public void goForward() {
+        goStraightAt( Math.abs(speed) );
+    }
+
+    /**
+     * Go backward at the current (negative) speed
+     */
+    public void goBackward() {
+        goStraightAt( - Math.abs(speed) );
+    }
+
+    /**
+     * Go forward at a specified speed
+     */
+    public void goForwardAt( int aspeed ) {
+        if( aspeed < 0 ) return;
+        goStraightAt( aspeed );
+    }
+
+    /**
+     * Go backward at a specified speed
+     */
+    public void goBackwardAt( int aspeed ) {
+        if( aspeed < 0 ) return;
+        goStraightAt( -aspeed );
+    }
+
+
+    //
+    // low-level movement and action
+    //
+
+    /**
+     * Move the Roomba via the low-level velocity + radius method.
+     * See the 'Drive' section of the Roomba ROI spec for more details.
+     * Low-level command.
+     * @param velocity  speed in millimeters/second, 
+     *                  positive forward, negative backward
+     * @param radius    radius of turn in millimeters
+     */
+    public void drive( int velocity, int radius ) {
+        byte cmd[] = { (byte)DRIVE,(byte)(velocity>>>8),(byte)(velocity&0xff), 
+                       (byte)(radius >>> 8), (byte)(radius & 0xff) };
+        logmsg("drive: "+hex(cmd[0])+","+hex(cmd[1])+","+hex(cmd[2])+","+
+               hex(cmd[3])+","+hex(cmd[4]));
+        send( cmd );
+    }
+
+    /**
+     * Play a musical note 
+     * Does it via the hacky method of defining a one-note song & playing it
+     * Uses up song slot 15.
+     * If another note is played before one is finished, the new note cuts off
+     * the old one.
+     * @param note     a note number from 31 (G0) to 127 (G8)
+     * @param duration duration of note in 1/64ths of a second
+     */
+    public void playNote( int note, int duration ) {
+        logmsg("playnote: "+note+":"+duration);
+        byte cmd[] = {
+            (byte)SONG, 3, 1, (byte)note, (byte)duration,  // define song
+            (byte)PLAY, 3 };                               // play it back
+        send( cmd );
+    }
+
+    public void playSong( int songnum ) {
+        byte cmd[] = { (byte)PLAY, (byte)songnum };
+        send(cmd);
+    }
+
+    /**
+     * Make a song
+     * @param songnum number of song to define
+     * @param song  array of songnotes, 
+     *              even entries are notenums, odd are duration of 1/6ths
+     */
+    public void createSong( int songnum, int song[] ) {
+        int len = song.length;
+        int songlen = len/2;
+        logmsg("createSong: songnum:"+songnum+", songlen:"+songlen);
+        byte cmd[] = new byte[len+3]; 
+        cmd[0] = (byte) SONG;
+        cmd[1] = (byte) songnum;
+        cmd[2] = (byte) songlen;
+        for( int i=0; i < len; i++ ) {
+            cmd[3+i] = (byte)song[i];
+        }
+        send(cmd);
+    }
+    /**
+     * Make a song
+     * @param songnum number of song to define
+     * @param song  array of Notes
+     */
+    public void createSong( int songnum, Note song[] ) {
+        int songlen = song.length;
+        logmsg("createSong: songnum:"+songnum+", songlen:"+songlen);
+        byte cmd[] = new byte[songlen+3]; 
+        cmd[0] = (byte) SONG;
+        cmd[1] = (byte) songnum;
+        cmd[2] = (byte) songlen;
+        int j=3;
+        for( int i=0; i < songlen; i++ ) {
+            cmd[j++] = (byte)song[i].notenum;
+            cmd[j++] = (byte)song[i].toSec64ths();
+        }
+        send(cmd);
+    }
+
+
+
+    /**
+     * Turns on/off the non-drive motors (main brush, vacuum, sidebrush).
+     * Sort of low-level.
+     * @param mainbrush  mainbrush motor on/off state
+     * @param vacuum     vacuum motor on/off state
+     * @param sidebrush  sidebrush motor on/off state
+     */
+    public void setMotors(boolean mainbrush,boolean vacuum,boolean sidebrush) {
+        byte cmd[] = { 
+            (byte)MOTORS, 
+            (byte)((mainbrush?0x04:0) | (vacuum?0x02:0) | (sidebrush?0x01:0))};
+        send( cmd );
+    }
+
+    /** 
+     * Turns on/off the various LEDs.
+     * Low-level command.
+     * FIXME: this is too complex
+     */
+    public void setLEDs( boolean status_green, boolean status_red, 
+                         boolean spot,boolean clean,boolean max,boolean dirt, 
+                         int power_color, int power_intensity ) {
+        int v = (status_green?0x20:0) | (status_red?0x10:0) | 
+            (spot?0x08:0) | (clean?0x04:0) | (max?0x02:0) | (dirt?0x01:0);
+        logmsg("setLEDS: "+binary(v));
+        byte cmd[] = { (byte)LEDS, (byte)v,
+                       (byte)power_color, (byte)power_intensity };
+        send(cmd);
+    }
+
+    //500 series
+    public void setLEDsOI( boolean checkRobot, boolean spot,boolean dock,boolean dirt, 
+        int power_color, int power_intensity ) {
+    	updateDisplay("setLEDsOI ("+checkRobot+")("+spot+")("+dock+")("+dirt+")("+power_color+")("+power_intensity+")", this.debug);
+    	int v = (checkRobot?0x08:0) | (dock?0x04:0) | (spot?0x02:0) | (dirt?0x01:0);
+    	logmsg("setLEDS: "+binary(v));
+    	byte cmd[] = { (byte)LEDS, (byte)v,
+    				   (byte)power_color, (byte)power_intensity };
+    	// TODO: find a way to do an updateDisplay with a byte array
+    	send(cmd);
+}
+    
+    /**
+     * Turn all vacuum motors on or off according to state
+     * @param state true to turn on vacuum function, false to turn it off
+     */
+    public void vacuum(boolean state) {
+        logmsg("vacuum: "+state);
+        setMotors(state,state,state);
+    }
+
+
+    //
+    // sensor functions
+    //
+
+
+    /**
+     * Compute possible safety fault.
+     * Called on every successful updateSensors().
+     * In normal use, call updateSensors() then check safetyFault().
+     * @return  true if indicates we had an event that took the Roomba out of
+     *          safe mode
+     * @see #updateSensors()
+     */
+    public boolean computeSafetyFault() {
+        safetyFault = (sensor_bytes[BUMPSWHEELDROPS] & WHEELDROP_MASK) != 0 ||
+            sensor_bytes[CLIFFLEFT]==1  || sensor_bytes[CLIFFFRONTLEFT]==1 ||
+            sensor_bytes[CLIFFRIGHT]==1 || sensor_bytes[CLIFFFRONTRIGHT]==1;
+
+        if( safetyFault && (mode == MODE_SAFE) ) mode = MODE_PASSIVE;
+
+        return safetyFault;
+    }
+
+    /** 
+     * Returns current connected state.  
+     * It's up to subclasses to ensure this variable is correct.
+     * @return current connected state
+     */
+    public boolean connected() { return connected; }
+
+    /** current ROI mode RoombaComm thinks the Roomba is in */
+    public int mode() { return mode; }
+    /** mode as String */
+    public String modeAsString() {
+        String s=null;
+        switch(mode) {
+        case MODE_UNKNOWN: s = "unknown"; break;
+        case MODE_PASSIVE: s = "passive"; break;
+        case MODE_SAFE:    s = "safe"; break;
+        case MODE_FULL:    s = "full"; break;
+        }
+        return s;
+    }
+
+    /** */
+    public boolean sensorsAutoUpdate() { return sensorsAutoUpdate; }
+    /** */
+    public void setSensorsAutoUpdate(boolean b) { sensorsAutoUpdate=b; }
+    /** */
+    public int sensorsUpdateTime() { return sensorsUpdateTime; }
+    /** */
+    public void setSensorsUpdateTime(int i) { sensorsUpdateTime=i; }
+
+    /**
+     *
+     */
+    public boolean safetyFault() { return safetyFault; } 
+
+    /**
+     * 
+     */
+    public boolean sensorsValid() {
+        // FIXME: 
+//        if( sensorsValid ) {  // may be valid but stale
+//            long difftime = System.currentTimeMillis() - sensorsLastUpdateTime;
+//            if( difftime > 2*sensorsUpdateTime ) { // give it some space
+//                return false;
+//            }
+//            else return true;
+//        }
+        return sensorsValid;
+    }
+    public String getSensorsAsString() {
+    	return sensorsAsString();
+    }
+    public String convertByteArrayToString(byte[] byteArray) {                
+        String value = new String(byteArray);        
+        return value;
+    }
+
+    
+    /** 
+     * @return all sensor data as a string
+     */
+    //* this likely needs to know about protocal to know how to read the sensors */
+    public String sensorsAsString() {
+	String sd="";
+    
+	if( debug ) {
+	    sd = "\n";
+	    for( int i=0; i<26; i++ )
+		sd += " "+hex(sensor_bytes[i]);
+	}
+        return
+            "*****\n" +
+            "bump:" + 
+            (bumpLeft()?"l":"_") + 
+            (bumpRight()?"r":"_") +
+            " wheel:" +
+            (wheelDropLeft()  ?"l":"_") +
+            (wheelDropCenter()?"c":"_") +
+            (wheelDropRight()  ?"r":"_") +
+            " wall:" + (wall() ?"Y":"n") + 
+            " cliff:" +
+            (cliffLeft()       ?"l":"_") +
+            (cliffFrontLeft()  ?"L":"_") +  
+            (cliffFrontRight() ?"R":"_") +
+            (cliffRight()      ?"r":"_") +
+            " dirtL:"+dirtLeft()+
+            " dirtR:"+dirtRight()+ "\n" +
+            "vwal:" + virtual_wall() +
+            " motr:" + motor_overcurrents() + 
+            " dirt:" + dirt_left() + "," + dirt_right() +
+            " remo:" + hex(remote_opcode()) +
+            " butt:" + hex(buttons()) +
+            " dist:" + distance() + 
+            " angl:" + angle() + "\n" +
+            "chst:" + charging_state() + 
+            " volt:" + voltage() +
+            " curr:" + current() +
+            " temp:" + temperatureF() + "F" +
+            " chrg:" + charge() +
+            " capa:" + capacity() +
+             sd ;
+    }
+    public String chargeDataAsString() {
+    	String sd="";
+    	if( debug ) {
+    	    sd = "\n";
+    	    for( int i=0; i<26; i++ )
+    		sd += " "+hex(sensor_bytes[i]);
+    	}
+            return
+            "Charging State: " + charging_state() + 
+                " Temperature: " + temperatureF() + "F\n" +
+                "Voltage:        " + voltage() +
+                " Current:     " + current() + "\n" +
+                "Capacity:       " + capacity() +
+                "  Charge:      " + charge() +
+                sd;
+        }
+    /** Did we bump into anything */
+    public boolean bump() {
+        return (sensor_bytes[BUMPSWHEELDROPS] & BUMP_MASK) !=0;
+    }
+    /** Left bump sensor */
+    public boolean bumpLeft() {
+        return (sensor_bytes[BUMPSWHEELDROPS] & BUMPLEFT_MASK) !=0;
+    }
+    /** Right bump sensor */
+    public boolean bumpRight() {
+        return (sensor_bytes[BUMPSWHEELDROPS] & BUMPRIGHT_MASK) !=0;
+    }
+    /** Left wheeldrop sensor */
+    public boolean wheelDropLeft() {
+        return (sensor_bytes[BUMPSWHEELDROPS] & WHEELDROPLEFT_MASK) !=0;
+    }
+    /** Right wheeldrop sensor */
+    public boolean wheelDropRight() {
+        return (sensor_bytes[BUMPSWHEELDROPS] & WHEELDROPRIGHT_MASK) !=0;
+    }
+    /** Center wheeldrop sensor */
+    public boolean wheelDropCenter() {
+        return (sensor_bytes[BUMPSWHEELDROPS] & WHEELDROPCENT_MASK) !=0;
+    }
+    /** Can we see a wall? */
+    public boolean wall() {
+        return sensor_bytes[WALL] != 0;
+    }
+
+    /**
+     * @return true if dirt present
+     */
+    public boolean dirt() {
+        int dl = sensor_bytes[DIRTLEFT] & 0xff;
+        int dr = sensor_bytes[DIRTRIGHT] & 0xff;
+        //if(debug) println("Roomba:dirt: dl,dr="+dl+","+dr);
+        return (dl > 100) || (dr > 100);
+    }
+    /**
+     * amount of dirt seen by left dirt sensor 
+     */
+    public int dirtLeft() {
+        return dirt_left();  // yeah yeah
+    }
+    /**
+     * amount of dirt seen by right dirt sensor 
+     */
+    public int dirtRight() {
+        return dirt_right();
+    }
+
+    /** left cliff sensor */
+    public boolean cliffLeft() {
+        return (sensor_bytes[CLIFFLEFT] != 0);
+    }  
+    /** front left cliff sensor */
+    public boolean cliffFrontLeft() {
+        return (sensor_bytes[CLIFFFRONTLEFT] != 0);
+    }  
+    /** front right cliff sensor */
+    public boolean cliffFrontRight() {
+        return (sensor_bytes[CLIFFFRONTRIGHT] != 0);
+    }  
+    /** right cliff sensor */
+    public boolean cliffRight() {
+        return sensor_bytes[CLIFFRIGHT] != 0;
+    }
+  
+    /** overcurrent on left drive wheel */
+    public boolean motorOvercurrentDriveLeft() {
+        return (sensor_bytes[MOTOROVERCURRENTS] & MOVERDRIVELEFT_MASK) != 0;
+    }
+    /** overcurrent on right drive wheel */
+    public boolean motorOvercurrentDriveRight() {
+        return (sensor_bytes[MOTOROVERCURRENTS] & MOVERDRIVERIGHT_MASK) != 0;
+    }
+    /** overcurrent on main brush */
+    public boolean motorOvercurrentMainBrush() {
+        return (sensor_bytes[MOTOROVERCURRENTS] & MOVERMAINBRUSH_MASK) != 0;
+    }
+    /** overcurrent on vacuum */
+    public boolean motorOvercurrentVacuum() {
+        return (sensor_bytes[MOTOROVERCURRENTS] & MOVERVACUUM_MASK) != 0;
+    }
+    /** overcurrent on side brush */
+    public boolean motorOvercurrentSideBrush() {
+        return (sensor_bytes[MOTOROVERCURRENTS] & MOVERSIDEBRUSH_MASK) !=0;
+    }
+
+    /** 'Power' button pressed state */
+    public boolean powerButton() {
+        return (sensor_bytes[BUTTONS] & POWERBUTTON_MASK) != 0;
+    }
+    /** 'Spot' button pressed state */
+    public boolean spotButton() {
+        return (sensor_bytes[BUTTONS] & SPOTBUTTON_MASK) != 0;
+    }
+    /** 'Clean' button pressed state */
+    public boolean cleanButton() {
+        return (sensor_bytes[BUTTONS] & CLEANBUTTON_MASK) != 0;
+    }
+    /** 'Max' button pressed state */
+    public boolean maxButton() {
+        return (sensor_bytes[BUTTONS] & MAXBUTTON_MASK) != 0;
+    }
+
+
+    //
+    // lower-level sensor access
+    //
+    /** lower-level func, returns raw byte */
+    public int bumps_wheeldrops() {
+        return sensor_bytes[BUMPSWHEELDROPS];
+    }
+    /** lower-level func, returns raw byte */
+    public int cliff_left() {
+        return sensor_bytes[CLIFFLEFT];
+    }
+    /** lower-level func, returns raw byte */
+    public int cliff_frontleft() {
+        return sensor_bytes[CLIFFFRONTLEFT];
+    }
+    /** lower-level func, returns raw byte */
+    public int cliff_frontright() {
+        return sensor_bytes[CLIFFFRONTRIGHT];
+    }
+    /** lower-level func, returns raw byte */
+    public int cliff_right() {
+        return sensor_bytes[CLIFFRIGHT];
+    }
+    /** lower-level func, returns raw byte */
+    public int virtual_wall() {
+        return sensor_bytes[VIRTUALWALL];
+    }
+    /** lower-level func, returns raw byte */
+    public int motor_overcurrents() {
+        return sensor_bytes[MOTOROVERCURRENTS];
+    }
+    /**  */
+    public int dirt_left() {
+        return sensor_bytes[DIRTLEFT] & 0xff;
+    }
+    /** */
+    public int dirt_right() {
+        return sensor_bytes[DIRTRIGHT] & 0xff;
+    }
+    /** lower-level func, returns raw byte */
+    public int remote_opcode() {
+        return sensor_bytes[REMOTEOPCODE];
+    }
+    /** lower-level func, returns raw byte */
+    public int buttons() {
+        return sensor_bytes[BUTTONS];
+    }
+
+    /** 
+     * Distance traveled since last requested
+     * units: mm
+     * range: -32768 - 32767
+     */
+    public short distance() {
+        return toShort(sensor_bytes[DISTANCE_HI],
+                       sensor_bytes[DISTANCE_LO]);
+    }
+    /** 
+     * Angle traveled since last requested
+     * units: mm, diff in distance traveled by two drive wheels
+     * range: -32768 - 32767
+     */
+    public short angle() {
+        return toShort(sensor_bytes[ANGLE_HI],
+                       sensor_bytes[ANGLE_LO]);
+    }  
+    /**
+     * angle since last read, but in degrees
+     */
+    // FIXME I think this should be (360 * angle())/(258 * PI)
+    public float angleInDegrees() {
+        return (float) angle() / millimetersPerDegree;
+    }
+    /**
+     * angle since last read, but in radians
+     */
+ // FIXME I think this should be (2 * angle())/258
+    public float angleInRadians() {
+        return (float) angle() / millimetersPerRadian;
+    }
+
+    /** 
+     * Charging state
+     * units: enumeration
+     * range: 
+     */
+    public int charging_state() {
+        return sensor_bytes[CHARGINGSTATE] & 0xff;
+    }
+    /** 
+     * Voltage of battery
+     * units: mV
+     * range: 0 - 65535
+     */
+    public int voltage() {
+        return toUnsignedShort(sensor_bytes[VOLTAGE_HI],
+                               sensor_bytes[VOLTAGE_LO]);
+    } 
+    /**
+     * Current flowing in or out of battery
+     * units: mA
+     * range: -332768 - 32767
+     */
+    public short current() {
+        return toShort(sensor_bytes[CURRENT_HI],
+                       sensor_bytes[CURRENT_LO]);
+    }
+    /**
+     * temperature of battery
+     * units: degrees Celcius
+     * range: -128 - 127
+     */
+    public byte temperature() {
+        return sensor_bytes[TEMPERATURE];
+    }
+    public byte temperatureF() {
+        int c = sensor_bytes[TEMPERATURE];
+        return (byte) ((9.0/5.0)*c + 32);
+    }    
+    /**
+     * Current charge of battery
+     * units: mAh 
+     * range: 0-65535
+     */
+    public int charge() {
+        return toUnsignedShort(sensor_bytes[CHARGE_HI],
+                               sensor_bytes[CHARGE_LO]);
+    }
+    /**
+     * Estimated charge capacity of battery
+     * units: mAh
+     * range: 0-65535
+     */
+    public int capacity() {
+        return toUnsignedShort(sensor_bytes[CAPACITY_HI],
+                               sensor_bytes[CAPACITY_LO]);
+    }
+
+    // possible modes
+    public static final int MODE_UNKNOWN = 0;
+    public static final int MODE_PASSIVE = 1;
+    public static final int MODE_SAFE    = 2;
+    public static final int MODE_FULL    = 3;
+
+    // Roomba ROI opcodes
+    // these should all be bytes, but Java bytes are signed, sucka
+    public static final int START   =  128;  // 0
+    public static final int BAUD    =  129;  // 1
+    public static final int CONTROL =  130;  // 0
+    public static final int SAFE    =  131;  // 0
+    public static final int FULL    =  132;  // 0
+    public static final int POWER   =  133;  // 0
+    public static final int SPOT    =  134;  // 0
+    public static final int CLEAN   =  135;  // 0
+    public static final int MAX     =  136;  // 0
+    public static final int DRIVE   =  137;  // 4
+    public static final int MOTORS  =  138;  // 1
+    public static final int LEDS    =  139;  // 3
+    public static final int SONG    =  140;  // 2N+2
+    public static final int PLAY    =  141;  // 1
+    public static final int SENSORS =  142;  // 1
+    public static final int DOCK    =  143;  // 0
+    public static final int PWMMOTORS = 144; // 3
+    public static final int DRIVEWHEELS = 145; 	// 4
+    public static final int DRIVEPWM = 146;  // 4
+    public static final int STREAM  =  148;  // N+1
+    public static final int QUERYLIST = 149; // N+1
+    public static final int STOPSTARTSTREAM = 150;  // 1
+    public static final int SCHEDULINGLEDS = 162; 	// 2
+    public static final int DIGITLEDSRAW = 163; 	// 4
+    public static final int DIGITLEDSASCII = 164;	// 4
+    public static final int BUTTONSCMD  =  165; // 1
+    public static final int SCHEDULE =  167;  // n
+    public static final int SETDAYTIME = 168; // 3
+
+    // offsets into sensor_bytes data
+    public static final int BUMPSWHEELDROPS     = 0;
+    public static final int WALL                = 1;
+    public static final int CLIFFLEFT           = 2;
+    public static final int CLIFFFRONTLEFT      = 3;
+    public static final int CLIFFFRONTRIGHT     = 4;
+    public static final int CLIFFRIGHT          = 5;
+    public static final int VIRTUALWALL         = 6;
+    public static final int MOTOROVERCURRENTS   = 7;
+    public static final int DIRTLEFT            = 8;
+    public static final int DIRTRIGHT           = 9;
+    public static final int REMOTEOPCODE        = 10;
+    public static final int BUTTONS             = 11;
+    public static final int DISTANCE_HI         = 12;
+    public static final int DISTANCE_LO         = 13;  
+    public static final int ANGLE_HI            = 14;
+    public static final int ANGLE_LO            = 15;
+    public static final int CHARGINGSTATE       = 16;
+    public static final int VOLTAGE_HI          = 17;
+    public static final int VOLTAGE_LO          = 18;  
+    public static final int CURRENT_HI          = 19;
+    public static final int CURRENT_LO          = 20;
+    public static final int TEMPERATURE         = 21;
+    public static final int CHARGE_HI           = 22;
+    public static final int CHARGE_LO           = 23;
+    public static final int CAPACITY_HI         = 24;
+    public static final int CAPACITY_LO         = 25;
+
+    // bitmasks for various thingems
+    public static final int WHEELDROP_MASK      = 0x1C;
+    public static final int BUMP_MASK           = 0x03;
+    public static final int BUMPRIGHT_MASK      = 0x01;
+    public static final int BUMPLEFT_MASK       = 0x02;
+    public static final int WHEELDROPRIGHT_MASK = 0x04;
+    public static final int WHEELDROPLEFT_MASK  = 0x08;
+    public static final int WHEELDROPCENT_MASK  = 0x10;
+
+    public static final int MOVERDRIVELEFT_MASK = 0x10;
+    public static final int MOVERDRIVERIGHT_MASK= 0x08;
+    public static final int MOVERMAINBRUSH_MASK = 0x04;
+    public static final int MOVERVACUUM_MASK    = 0x02;
+    public static final int MOVERSIDEBRUSH_MASK = 0x01;
+
+    public static final int POWERBUTTON_MASK    = 0x08;  
+    public static final int SPOTBUTTON_MASK     = 0x04;  
+    public static final int CLEANBUTTON_MASK    = 0x02;  
+    public static final int MAXBUTTON_MASK      = 0x01;  
+
+    // which sensor packet, argument for sensors(int)
+    public static final int SENSORS_ALL         = 0;
+    public static final int SENSORS_PHYSICAL    = 1;
+    public static final int SENSORS_INTERNAL    = 2;
+    public static final int SENSORS_POWER       = 3;
+
+    public static final int REMOTE_NONE         = 0xff;
+    public static final int REMOTE_POWER        = 0x8a;
+    public static final int REMOTE_PAUSE        = 0x89;
+    public static final int REMOTE_CLEAN        = 0x88;
+    public static final int REMOTE_MAX          = 0x85;
+    public static final int REMOTE_SPOT         = 0x84;
+    public static final int REMOTE_SPINLEFT     = 0x83;
+    public static final int REMOTE_FORWARD      = 0x82;
+    public static final int REMOTE_SPINRIGHT    = 0x81;
+
+        /*
+no button = -1
+power = -118 8a 
+pause = -119 89
+clean = -120 88
+max = -123 85
+spot = -124 84
+spinleft = -125 81  (8d keyup?)
+forward = -126  82   (8c?)
+spinright = -127 83
+        */
+
+    //
+    // utility methods
+    //
+
+    /**
+     *
+     */
+    static public final short toShort(byte hi, byte lo) {
+        return (short)((hi << 8) | (lo & 0xff));
+    }
+    /**
+     *
+     */
+    static public final int toUnsignedShort(byte hi, byte lo) {
+        return (int)(hi & 0xff) << 8 | lo & 0xff;
+    }
+    
+    public void println(String s) {
+        System.out.println(s);
+    }
+    
+    public String hex(byte b) {
+        return Integer.toHexString(b&0xff);
+    }
+
+    public String hex(int i) {
+        return Integer.toHexString(i);
+    }
+    
+    
+    public String binary(int i) {
+        return Integer.toBinaryString(i);
+    }
+
+    /**
+     * just a little debug 
+     */
+    public void logmsg(String msg) {
+        if(debug)
+        {
+            System.err.println("RoombaComm ("+System.currentTimeMillis()+"):"+msg);
+        	System.err.flush();
+        }
+    }
+
+    /**
+     * General error reporting, all corraled here just in case
+     * I think of something slightly more intelligent to do.
+     */
+    public void errorMessage(String where, Throwable e) {
+        e.printStackTrace();
+        throw new RuntimeException("Error inside Serial." + where + "()");
+    }
+
+	public String getProtocol() {
+		return protocol;
+	}
+
+	public void setProtocol(String protocol) {
+		if (protocol.equals("SCI")) {
+			rate = 57600;
+		} else if (protocol.equals("OI")) {
+			rate = 115200;
+		}
+		this.protocol = protocol;
+		logmsg("Protocol: " + protocol +" , rate: "+rate);
+		writeConfigFile(portname, protocol, waitForDSR?'Y':'N');
+	}
+
+	/**
+	 * Write a config file with current settings
+	 */
+	protected void writeConfigFile(String port, String protocol, char waitForDSR) {
+		try {
+			FileWriter f = new FileWriter(".roomba_config", false);
+	    	BufferedWriter w = new BufferedWriter(f); // create file
+	    	if (port != null){
+	    		w.write(port);
+	    	}else{
+	    		w.newLine();
+	    	}
+	    	w.newLine();
+	    	if (protocol != null){
+	    		w.write(protocol);
+	    	}else{
+	    		w.newLine();
+	    	}
+	    	w.newLine();
+	    	w.write(waitForDSR);
+	    	w.newLine();
+	    	w.close();
+	    	f.close();
+		} catch (IOException e) {
+			logmsg("unable to write .roomba_config " + e);
+		}
+	}
+
+	protected void readConfigFile() {
+		try {
+			FileReader f = new FileReader(".roomba_config");
+			BufferedReader r = new BufferedReader(f);
+			portname = r.readLine();
+			setProtocol(r.readLine());
+			if (getProtocol().equals("SCI")) {
+				rate = 57600;
+			}else if (getProtocol().equals("OI")) {
+				rate = 115200;
+			}
+			waitForDSR = r.readLine().equals("Y")?true:false;
+			logmsg("read config port: " + serialPort + " protocol: " + getProtocol() + " waitDSR: " + waitForDSR);    		
+		} catch (IOException e) {
+			logmsg("unable to read .roomba_config " + e);
+		}
+	}
+
+	public void setLEDs(RoombaComm roombacomm) {
+		if( !roombacomm.connected() ){
+			updateDisplay("not-connected", this.debug);
+			return;
+		}
+		updateDisplay("setLEDs protocal is ("+this.protocol+")", this.debug);
+		if (this.protocol.equalsIgnoreCase("SCI")){
+		roombacomm.setLEDs(this.greenOn, this.redOn, this.toggleSpot, this.toggleClean, this.toggleMax, this.toggleDirt, 
+				this.power_color, this.power_int);
+		}
+		if (this.protocol.equalsIgnoreCase("OI")){
+			roombacomm.setLEDsOI(this.toggleCheckRobot, this.toggleSpot, this.toggleDock, this.toggleDirt, this.power_color, this.power_int);
+			updateDisplay("Checkrobot("+this.toggleCheckRobot +"),Spot("+ this.toggleSpot +"),Dock("+ this.toggleDock +"),Dirt("+ this.toggleDirt +"),Pcolor("+ this.power_color +"),Pint("+ this.power_int+")",true);
+		}
+	}
+
+	public void setChgGreenLED(RoombaComm roombacomm, boolean green) {
+		this.greenOn=green;
+		updateDisplay("setChgGreenLED", true);
+		this.setLEDs(roombacomm);
+	}
+
+	public void setChgRedLED(RoombaComm roombacomm, boolean red) {
+		this.redOn=red;
+		updateDisplay("setChgRedLED", true);
+		this.setLEDs(roombacomm);
+	}
+
+	public void setChgSpotLED(RoombaComm roombacomm, boolean spot) {
+		this.toggleSpot=spot;
+		updateDisplay("setChgSpotLED value("+spot+")", true);
+		this.setLEDs(roombacomm);
+	}
+
+	public void setChgCleanLED(RoombaComm roombacomm, boolean clean) {
+		updateDisplay("setChgCleanLED",true);
+		this.toggleClean=clean;
+		this.setLEDs(roombacomm);
+	}
+
+	public void setChgMaxLED(RoombaComm roombacomm, boolean max) {
+		updateDisplay("setChgMaxLED",true);
+		this.toggleMax=max;
+		this.setLEDs(roombacomm);
+	}
+
+	public void setChgDirtLED(RoombaComm roombacomm, boolean dirt) {
+		updateDisplay("setChgDirtLED",true);
+		this.toggleDirt=dirt;
+		this.setLEDs(roombacomm);
+	}
+
+	public void setChgPowerColorLED(RoombaComm roombacomm, int power_color) {
+		updateDisplay("setChgPowerColorLED",true);
+		this.power_color=power_color;
+		this.setLEDs(roombacomm);
+	}
+
+	public void setChgPowerIntensityLED(RoombaComm roombacomm, int power_intensity) {
+		updateDisplay("setChgPowerIntensityLED",true);
+		this.power_int=power_intensity;
+		this.setLEDs(roombacomm);
+	}
+	public void setChgCheckRobotLED(RoombaComm roombacomm, boolean toggleCheckRobot) {
+		this.toggleCheckRobot = toggleCheckRobot;
+		this.setLEDs(roombacomm);
+	}
+
+	public void setChgDockLED(RoombaComm roombacomm, boolean toggleDock) {
+		this.toggleDock = toggleDock;
+		this.setLEDs(roombacomm);
+	}
+	protected void updateDisplay(String s, boolean onlyDebug) {
+	    if (onlyDebug && debug){
+	    	updateDisplay(s);
+	    	System.out.println(s);
+	    }
+	}
+
+	protected void updateDisplay(String s) {
+	//      displayText.append( s );
+	//      displayText.setCaretPosition(displayText.getDocument().getLength());
+	  }
+
+	public boolean isRedOn() {
+		return redOn;
+	}
+
+	public boolean isGreenOn() {
+		return greenOn;
+	}
+
+	public boolean isToggleSpot() {
+		return toggleSpot;
+	}
+
+	public boolean isToggleClean() {
+		return toggleClean;
+	}
+
+	public boolean isToggleMax() {
+		return toggleMax;
+	}
+
+	public boolean isToggleDirt() {
+		return toggleDirt;
+	}
+
+	public boolean isToggleCheckRobot() {
+		return toggleCheckRobot;
+	}
+
+
+	public boolean isToggleDock() {
+		return toggleDock;
+	}
+
+
+	/**
+	 * Returns the number of bytes that have been read from serial
+	 * and are waiting to be dealt with by the user.
+	 * (from processing.serial.Serial)
+	 *
+	private int available() {
+	    return (bufferLast - bufferIndex);
+	}
+	
+	/**
+	 * Return a byte array of anything that's in the serial buffer.
+	 * Not particularly memory/speed efficient, because it creates
+	 * a byte array on each read, but it's easier to use than
+	 * readBytes(byte b[]) (see below).
+	 * (from processing.serial.Serial)
+	 *
+	private byte[] readBytes() {
+	    if (bufferIndex == bufferLast) return null;
+	
+	    synchronized (buffer) {
+	        int length = bufferLast - bufferIndex;
+	        byte outgoing[] = new byte[length];
+	        System.arraycopy(buffer, bufferIndex, outgoing, 0, length);
+	
+	        bufferIndex = 0;  // rewind
+	        bufferLast = 0;
+	        return outgoing;
+	    }
+	}
+	  
+	/**
+	 * Grab whatever is in the serial buffer, and stuff it into a
+	 * byte buffer passed in by the user. This is more memory/time
+	 * efficient than readBytes() returning a byte[] array.
+	 *
+	 * Returns an int for how many bytes were read. If more bytes
+	 * are available than can fit into the byte array, only those
+	 * that will fit are read.
+	 * (from processing.serial.Serial)
+	 *
+	public int readBytes(byte outgoing[]) {
+	    if (bufferIndex == bufferLast) return 0;
+	
+	    synchronized (buffer) {
+	        int length = bufferLast - bufferIndex;
+	        if (length > outgoing.length) length = outgoing.length;
+	        System.arraycopy(buffer, bufferIndex, outgoing, 0, length);
+	
+	        bufferIndex += length;
+	        if (bufferIndex == bufferLast) {
+	            bufferIndex = 0;  // rewind
+	            bufferLast = 0;
+	        }
+	        return length;
+	    }
+	}
+	*/
+	public void powerOn() {
+	        logmsg("powerOn");
+	        mode = MODE_PASSIVE;
+	//        MSComm1.Output = "+++" & Chr(13)
+	//        MSComm1.Output = "ATSW22,6,1,1" & Chr(13)
+	//        MSComm1.Output = "ATSW23,6,0,1" & Chr(13)
+	//        MSComm1.Output = "ATSW23,6,1,1" & Chr(13)
+	//        MSComm1.Output = "ATMD" & Chr(13
+	        send( ("+++"+(char)13).getBytes());
+	        send( ("ATSW22,6,1,1"+(char)13).getBytes());
+	        send( ("ATSW23,6,0,1"+(char)13).getBytes());
+	        send( ("ATSW23,6,1,1"+(char)13).getBytes());
+	        send( ("ATMD"+(char)13).getBytes());
+	//: TYPE : ATSW22,6,0,1<cr> ; First change it to high
+	//REPLY: <cr_lf>OK<cr_lf>
+	//TYPE : ATSW22,6,0,0<cr> ; Change it to low
+	//REPLY: <cr_lf>OK<cr_lf>
+	//TYPE : ATSW22,6,0,1<cr>
+	//REPLY: <cr_lf>OK<cr_lf> ; Change it to high
+	//        send( "+++\n".getBytes());
+	//        send( "ATSW22,6,0,1\n".getBytes());
+	//        send( "ATSW22,6,0,0\n".getBytes());
+	//        send( "ATSW22,6,0,1\n".getBytes());
+	//        send( "ATMD\n".getBytes());
+	        
+	}
+
+	public byte[] getSensor_bytes() {
+		return sensor_bytes;
+	}
+}