added stuff

1 files changed, 505 insertions, 0 deletions

diff --git a/roombacomm-client/src/com/hackingroomba/roombacomm/RoombaCommSerial.java b/roombacomm-client/src/com/hackingroomba/roombacomm/RoombaCommSerial.java
new file mode 100644
index 0000000..c768baf
--- /dev/null
+++ b/roombacomm-client/src/com/hackingroomba/roombacomm/RoombaCommSerial.java
@@ -0,0 +1,505 @@
+/*
+ *  RoombaComm Serial 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.*;
+import java.io.*;
+import java.util.*;
+
+
+/**
+ *  The serial-port based implementation of RoombaComm.
+ *  Handles both physical RS-232 ports, USB adapter ports like Keyspan 
+ *  USA-19HS, and Bluetooth serial port profiles.
+ * 
+ * <p> Some code taken from processing.serial.Serial.  Thanks guys! </p>
+ *
+ *  The interaction model for setting the port and protocol and WaitForDSR parameters is as follows.
+ *  <p>
+ *  On creation, the class initializes the parameters, then tries to read .roomba_config.
+ *  If it can read the config file and parse out the parameters, it sets the parameters to
+ *  the values in the config file. Apps can read the current settings for display using methods
+ *  on the class. Apps can override the settings by accepting user input and setting the
+ *  parameters using methods on the class, or the connect() method. Parameters that are
+ *  changed by the app are re-written in the config file, for use as defaults next run.
+ *  Command-line apps can make these parameters optional, by using the defaults if the
+ *  user doesn't specify them.
+ *  
+ *  SVN id value is $Id: RoombaCommSerial.java 136 2010-04-06 00:07:23Z black.123 $  
+ */
+public class RoombaCommSerial extends RoombaComm implements SerialPortEventListener
+{
+static final int databits = 8;
+    static final int parity   = SerialPort.PARITY_NONE;
+    static final int stopbits = SerialPort.STOPBITS_1;
+    /**
+     * The time to wait in milliseconds after sending sensors command before
+     * attempting to read
+     */
+    public static int updateSensorsPause = 400;
+
+    /** the serial input stream, normally you don't need access to this */
+    public InputStream input;
+    /** the serial output stream, normally you don't need access to this */
+    public OutputStream output;
+
+    /** 
+     * RXTX bombs when flushing output sometimes, so by default do not
+     * flush the output stream.  If the output is too buffered to be 
+     * useful, do:  
+     *  roombacomm.comm.flushOutput = true;
+     * before using it and see if it works.
+     */
+    public boolean flushOutput = false;
+    byte buffer[] = new byte[32768];
+    int bufferLast;
+
+    //int bufferSize = 26;  // how big before reset or event firing
+    //boolean bufferUntil;
+    //int bufferUntilByte;
+
+    /**
+     * Let you check to see if a port is in use by another Rooomba
+     * before trying to use it.
+     */
+    public static boolean isPortInUse( String pname ) {
+    	if (ports != null && pname != null){
+    		Boolean inuse = (Boolean) ports.get( pname );
+    		if( inuse !=null ) {
+    			return inuse.booleanValue();
+    		}
+    	}
+        return false;
+    }
+
+    // constructor
+    public RoombaCommSerial() {
+        super();
+        makePorts();
+        readConfigFile();
+    }
+    public RoombaCommSerial(boolean autoupdate) {
+        super(autoupdate);
+        makePorts();
+        readConfigFile();
+    }
+    public RoombaCommSerial(boolean autoupdate, int updateTime) {
+        super(autoupdate, updateTime);
+        makePorts();
+        readConfigFile();
+    }
+
+    void makePorts() {
+        if( ports == null ) 
+            ports = Collections.synchronizedMap(new TreeMap());
+    }
+    /**
+     * Connect to a serial port specified by portid
+     * doesn't guarantee connection to Roomba, just to serial port
+     * @param portid name of port, e.g. "/dev/cu.KeySerial1" or "COM3"
+     * @return true if connect was successful, false otherwise
+     */
+    public boolean connect(String portid) {
+        logmsg("connecting to port '"+portid+"'");
+        portname = portid;
+		writeConfigFile(portname, getProtocol(), waitForDSR?'Y':'N');
+
+        if( isPortInUse( portid ) ) {
+            logmsg("port is in use");
+            return false;
+        }
+
+        connected = open_port();
+
+        if( connected ) {
+            // log in the global ports hash if the port is in use now or not
+            ports.put( portname, new Boolean( connected ) );
+            sensorsValid = false;
+        }
+        else {
+            disconnect();
+        }
+        
+        return connected;
+    }
+    public boolean connect(String portid,String protocal) {
+    	String setProt="OI";
+    	if (protocal.equalsIgnoreCase("SCI")){
+    		setProt="SCI";
+    	}
+    	if (protocal.equalsIgnoreCase("OI")){
+    		setProt="OI";
+    	}
+    	setProtocol(setProt);
+        logmsg("connecting to port '"+portid+"' using protocal '"+setProt+"'");
+        return connect(portid);
+    }
+
+    /**
+     * Disconnect from serial port
+     */
+    public void disconnect() {
+        connected = false;
+
+        // log in the global ports hash if the port is in use now or not
+        if (ports != null && portname != null ){
+        	ports.put( portname, new Boolean( connected ) );
+        }
+
+        try {
+            // do io streams need to be closed first?
+            if (input != null) input.close();
+            if (output != null) output.close();
+        } catch (Exception e) {
+            e.printStackTrace();
+        }
+        input = null;
+        output = null;
+    
+        try {
+            if (serialPort != null) serialPort.close();  // close the port 
+        } catch (Exception e) {
+            e.printStackTrace();
+        }
+        serialPort = null;
+    }
+
+    /**
+     * subclassed.  FIXME: 
+     */
+    public boolean send(byte[] bytes) {
+        try {
+            output.write(bytes);
+            if( flushOutput ) output.flush();   // hmm, not sure if a good idea
+        } catch (Exception e) { // null pointer or serial port dead
+            e.printStackTrace();
+        }
+        return true;
+    }
+
+    /**
+     * This will handle both ints, bytes and chars transparently.
+     */
+    public boolean send(int b) {  // will also cover char or byte
+        try {
+            output.write(b & 0xff);  // for good measure do the &
+            if( flushOutput ) output.flush();   // hmm, not sure if a good idea
+        } catch (Exception e) { // null pointer or serial port dead
+            //errorMessage("send", e);
+            e.printStackTrace();
+        }
+        return true;
+    }
+
+    /**
+     * toggles DD line via serial port DTR  (if available)
+     */
+    public void wakeup() {
+        serialPort.setDTR(false);
+        pause(500);
+        serialPort.setDTR(true);
+    }
+  
+    /**
+     * Update sensors.  Block for up to 1000 ms waiting for update
+     * To use non-blocking, call sensors() and then poll sensorsValid()
+     */
+    public boolean updateSensors() {
+    	System.out.println("updateSensors-start");
+        sensorsValid = false;
+        sensors(); // requests sensor group 0
+        for(int i=0; i < 20; i++) {
+            if( sensorsValid ) { 
+            	if ((buffer[1] > 1) || (buffer[1] < 0)) {
+            		sensorsValid = false;
+            		logmsg("updateSensors: received invalid data while attempting to read Roomba sensors!");
+            		System.out.println("updateSensors: received invalid data while attempting to read Roomba sensors!");
+            	} else {
+            		logmsg("updateSensors: sensorsValid!");
+            		System.out.println("updateSensors: sensorsValid!");
+            	}
+                
+                break;
+            }
+            logmsg("updateSensors: pausing...");
+            System.out.println("updateSensors: pausing...");
+            pause( 50 );
+        }
+	if (!sensorsValid){
+        	logmsg("updateSensors: no data received.");
+        };
+	System.out.println("updateSensors: end");
+        return sensorsValid;
+    }
+    
+    /**
+     * Update sensors.  Block for up to 1000 ms waiting for update
+     * To use non-blocking, call sensors() and then poll sensorsValid()
+     */
+    public boolean updateSensors(int packetcode) {
+        sensorsValid = false;
+        sensors(packetcode);
+        for(int i=0; i < 20; i++) {
+            if( sensorsValid ) { 
+                logmsg("updateSensors: sensorsValid!");
+                break;
+            }
+            logmsg("updateSensors: pausing...");
+            pause( 50 );
+        }
+
+        return sensorsValid;
+    }
+
+    /**
+     * called by serialEvent when we have enough bytes to make sensors valid
+     */
+    public void computeSensors() {
+        sensorsValid = true;
+        sensorsLastUpdateTime = System.currentTimeMillis();
+        computeSafetyFault();
+    }
+        /*
+        pause(updateSensorsPause);     // take a breather to let data come back
+        sensorsValid = false;          // assume the worst, we're gothy
+        int n = available();
+        //logmsg("updateSensors:n="+n);
+        if( n >= 26) {                 // there are enough bytes to read
+            n = readBytes(sensor_bytes);
+            if( n==26 ) {              // did we get enough?
+                sensorsValid = true;   // then everything's good, otherwise bad
+                computeSafetyFault();
+            }
+        } else {
+            logmsg("updateSensors:only "+n+" bytes available, not updating sensors");
+        }
+        
+        //logmsg("buffer contains: "+ buffer );
+        return sensorsValid;
+        */
+
+    /**
+     * If this just hangs and never completes on Windows,
+     * it may be because the DLL doesn't have its exec bit set.
+     * Why the hell that'd be the case, who knows.
+     * FIXME: deal more gracefully
+     * (from processing.serial.Serial)
+     */
+    public String[] listPorts() {
+        Map ps = Collections.synchronizedMap(new LinkedHashMap());
+        //Vector list = new Vector();
+        try {
+            //System.err.println("trying");
+        	Enumeration portList=null;
+        	try {
+        		portList = CommPortIdentifier.getPortIdentifiers();
+        	} catch (Exception e) {
+                //System.err.println("2");
+                errorMessage("listPorts1", e);
+            }
+            //System.err.println("got port list");
+            while (portList.hasMoreElements()) {
+                CommPortIdentifier portId = (CommPortIdentifier) portList.nextElement();
+                logmsg("Found port: " + portId.getName());
+                
+                if (portId.getPortType() == CommPortIdentifier.PORT_SERIAL) {
+                    String name = portId.getName();
+                    //list.addElement(name);
+                    Boolean state = (Boolean) ports.get( name );
+                    if( state==null ) state = new Boolean(false);
+                    ps.put( name, state );
+                }
+            }
+        } catch (UnsatisfiedLinkError e) {
+            //System.err.println("1");
+            errorMessage("listPorts", e);
+        } catch (Exception e) {
+            //System.err.println("2");
+            errorMessage("listPorts", e);
+        }
+        //System.err.println("move out");
+        /*
+        for( Enumeration e = list.elements(); e.hasMoreElements(); ) {
+            String p = (String) e.nextElement();
+            if( ! ports.containsKey( p ) ) {
+                ports.put( p, new Boolean(false) );
+            }
+        }
+
+        // DEBUG
+        System.err.println("ports hashtable:");
+        for( Enumeration e = ports.keys(); e.hasMoreElements(); ) {
+            String p = (String) e.nextElement();
+            Boolean b = (Boolean) ports.get(p);
+            System.err.println("port:"+p+", inuse:"+b);
+        }
+        */
+        ports = ps;
+        String outgoing[] = 
+            (String[]) new TreeSet(ports.keySet()).toArray(new String[0]);
+
+        return outgoing;
+    }
+
+
+    public boolean isWaitForDSR() {
+		return waitForDSR;
+	}
+
+	public void setWaitForDSR(boolean waitForDSR) {
+		this.waitForDSR = waitForDSR;
+		writeConfigFile(portname, getProtocol(), waitForDSR?'Y':'N');
+	}
+
+	public String getPortname() {
+		return portname;
+	}
+	
+	public void setPortname(String p) {
+		portname = p;
+		logmsg("Port: " + portname);
+		writeConfigFile(portname, getProtocol(), waitForDSR?'Y':'N');
+
+	}
+
+   // -------------------------------------------------------------
+
+    // below only used internally to this class
+    // -------------------------------------------------------------
+
+	/**
+     * internal method, used by connect()
+     * FIXME: make it faile more gracefully, recognize bad port
+     */
+    private boolean open_port() {
+        boolean success = false;
+        try {
+            Enumeration portList = CommPortIdentifier.getPortIdentifiers();
+            while (portList.hasMoreElements()) {
+                CommPortIdentifier portId =
+                    (CommPortIdentifier) portList.nextElement();
+                
+                if (portId.getPortType() == CommPortIdentifier.PORT_SERIAL) {
+                    System.out.println("found " + portId.getName());
+                    if (portId.getName().equals(portname)) {
+                        logmsg("open_port:"+ portId.getName());
+                        serialPort = (SerialPort)portId.open("roomba serial", 2000);
+                        //port.setFlowControlMode(SerialPort.FLOWCONTROL_NONE);
+                        input  = serialPort.getInputStream();
+                        output = serialPort.getOutputStream();
+                        serialPort.setSerialPortParams(rate,databits,stopbits,parity);
+                        serialPort.addEventListener(this);
+                        serialPort.notifyOnDataAvailable(true);
+                        logmsg("port "+portname+" opened successfully");
+
+                        if( waitForDSR ) {
+                            int i=40;
+                            while( !serialPort.isDSR() && i-- != 0) {
+                                logmsg("DSR not ready yet");
+                                pause(150); // 150*40 = 6 seconds
+                            }
+                            success = serialPort.isDSR();
+                        } else {
+                            success = true;
+                        }
+                    }
+                }
+            }
+      
+        } catch (Exception e) {
+            logmsg("connect failed: "+e);
+            serialPort = null;
+            input = null;
+            output = null;
+        }
+                        
+        return success;
+    }
+
+    /**
+     * callback for SerialPortEventListener
+     * (from processing.serial.Serial)
+     */
+    synchronized public void serialEvent(SerialPortEvent serialEvent) {
+        try {
+        logmsg("serialEvent:"+serialEvent+", Available:"+input.available());
+        if (serialEvent.getEventType() == SerialPortEvent.DATA_AVAILABLE) {
+                while (input.available() > 0) {
+                    logmsg("serialEvent: available="+input.available());
+                    buffer[bufferLast++] = (byte) input.read();
+                    if( bufferLast == 26 ) {
+                        bufferLast = 0;
+                        System.arraycopy(buffer, 0, sensor_bytes, 0, 26);
+                        computeSensors();
+                    }
+                    /*
+                    synchronized (buffer) {
+                        if (bufferLast == buffer.length) {
+                            byte temp[] = new byte[bufferLast << 1];
+                            System.arraycopy(buffer, 0, temp, 0, bufferLast);
+                            buffer = temp;
+                        }
+                        buffer[bufferLast++] = (byte) input.read();
+                    }
+                    */
+                } // while
+        }
+        } catch (IOException e) {
+            errorMessage("serialEvent", e);
+        }
+    }
+    
+    @SuppressWarnings("unchecked")
+	public void playSong(RoombaComm roombacomm, String rtttl){
+    	ArrayList notelist = RTTTLParser.parse( rtttl );
+        int songsize = notelist.size();
+        // if within the size of a roomba song,  make the nsong, then play
+        if( songsize <= 16 ) {
+        	updateDisplay("creating a song with createSong()", true);
+        	int notearray[] = new int[songsize*2];
+            int j=0;
+            for( int i=0; i< songsize; i++ ) {
+                Note note = (Note) notelist.get(i);
+                int sec64ths = note.duration * 64/1000;
+                notearray[j++] = note.notenum;
+                notearray[j++] = sec64ths;
+            }
+            roombacomm.createSong( 1, notearray );
+            roombacomm.playSong( 1 );
+        }
+        // otherwise, try to play it in realtime
+        else {
+        	updateDisplay("playing song in realtime with playNote()",true);
+            int fudge = 20;
+            for( int i=0; i< songsize; i++ ) {
+                Note note = (Note) notelist.get(i);
+                int duration = note.duration;
+                int sec64ths = duration*64/1000;
+                if( sec64ths < 5 ) sec64ths = 5;
+                if( note.notenum != 0 )
+                    roombacomm.playNote( note.notenum, sec64ths );
+                roombacomm.pause( duration + fudge );
+            }
+        }
+    }	
+}