0,0 → 1,601 |
/* |
AVRcamVIEW: A PC application to test out the functionallity of the |
AVRcam real-time image processing engine. |
Copyright (C) 2004 Brent A. Taylor |
|
This program is free software; you can redistribute it and/or |
modify it under the terms of the GNU General Public |
License as published by the Free Software Foundation; either |
version 2 of the License, or (at your option) any later version. |
|
This program 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 |
General Public License for more details. |
|
You should have received a copy of the GNU General Public |
License along with this program; if not, write to the Free Software |
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
|
For more information on the AVRcamVIEW, please contact: |
|
taylorba@comcast.net |
|
or go to www.jrobot.net for more details regarding the system. |
*/ |
|
package avr.device; |
|
import java.io.*; |
import java.nio.*; |
import java.util.*; |
import javax.comm.*; |
|
import java.awt.*; |
|
import avr.connection.*; |
import avr.connection.event.*; |
import avr.device.event.*; |
import avr.io.*; |
import avr.lang.*; |
|
public class Device implements ConnectionListener { |
|
public static void main(String[] args) throws Exception { |
Device device = new Device(); |
|
SerialParams params = new SerialParams(); |
|
device.setConnection(new SerialConnection("COM1", params)); |
|
device.connect(); |
device.sendPing(); |
device.sendDumpFrame(); |
device.sendGetVersion(); |
device.sendSetRegisters(new HashMap()); |
device.sendReset(); |
device.sendEnableTracking(); |
device.sendDisableTracking(); |
device.sendSetColorMap(null, null, null); |
|
try { |
System.in.read(); |
} catch(IOException ioe) { |
ioe.printStackTrace(); |
} |
device.disconnect(); |
|
} |
|
private static final String ACK = "ACK"; |
private static final String NCK = "NCK"; |
private static final String VERSION = "AVR"; |
|
private static final String COLOR_MAP_KEY = "avr.color.map"; |
|
// timeout after 3 seconds |
private static final int RESPONSE_TIMEOUT = 3000; |
|
private java.util.List connectionListeners; |
private java.util.List dataListeners; |
private AbstractConnection con; |
|
private InputStream in; |
private OutputStream out; |
|
private int[][] colorMap; |
private Color[] mapColors; |
|
private Timer responseTimer; |
private TimerTask responseTask; |
|
public Device() { |
connectionListeners = new ArrayList(3); |
dataListeners = new ArrayList(3); |
loadMap(); |
|
responseTimer = new Timer(); |
} |
|
public Color[] getMapColors() { |
return mapColors; |
} |
|
public int[][] getColorMap() { |
return colorMap; |
} |
|
public void setColorMap(int[][] colorMap) { |
this.colorMap = colorMap; |
setMapColors(); |
saveMap(); |
} |
|
public void saveMap() { |
try { |
ByteArrayOutputStream bOut = new ByteArrayOutputStream(); |
DataOutputStream dOut = new DataOutputStream(bOut); |
|
dOut.writeInt(colorMap.length); |
for(int i = 0; i < colorMap.length; i++) { |
dOut.writeInt(colorMap[i].length); |
for(int j = 0; j < colorMap[i].length; j++) { |
dOut.writeInt(colorMap[i][j]); |
} |
} |
|
dOut.close(); |
|
AVRSystem.PREFS.putByteArray(COLOR_MAP_KEY, bOut.toByteArray()); |
|
} catch(Exception e) { |
e.printStackTrace(); |
} |
} |
|
public void loadMap() { |
try { |
byte[] data = AVRSystem.PREFS.getByteArray(COLOR_MAP_KEY, null); |
if(data != null) { |
ByteArrayInputStream bIn = new ByteArrayInputStream(data); |
DataInputStream dIn = new DataInputStream(bIn); |
|
int width = dIn.readInt(); |
colorMap = new int[width][]; |
for(int i = 0; i < colorMap.length; i++) { |
colorMap[i] = new int[dIn.readInt()]; |
for(int j = 0; j < colorMap[i].length; j++) { |
colorMap[i][j] = dIn.readInt(); |
} |
} |
|
dIn.close(); |
} else { |
colorMap = new int[3][AVRSystem.NUM_INTENSITIES]; |
} |
|
setMapColors(); |
|
} catch(Exception e) { |
e.printStackTrace(); |
} |
|
} |
|
private void setMapColors() { |
mapColors = new Color[8]; |
|
for(int col = 0; col < mapColors.length; col++) { |
int value = 0; |
|
int red = 0; |
int green = 0; |
int blue = 0; |
|
int numRed = 0; |
int numGreen = 0; |
int numBlue = 0; |
|
|
for(int i = 0; i < AVRSystem.NUM_INTENSITIES; i++) { |
|
if((colorMap[0][i] & (0x01 << (7 - col))) != 0) { |
red += i << 4; |
numRed++; |
} |
|
if((colorMap[1][i] & (0x01 << (7 - col))) != 0) { |
green += i << 4; |
numGreen++; |
} |
|
if((colorMap[2][i] & (0x01 << (7 - col))) != 0) { |
blue += i << 4; |
numBlue++; |
} |
|
} |
|
if(numRed > 0) { |
red /= numRed; |
} |
|
if(numGreen > 0) { |
green /= numGreen; |
} |
|
if(numBlue > 0) { |
blue /= numBlue; |
} |
|
value = (red << 16) | (green << 8) | blue; |
|
mapColors[col] = new Color(value); |
} |
|
} |
|
public AbstractConnection getConnection() { |
return con; |
} |
|
public void setConnection(AbstractConnection con) { |
this.con = con; |
} |
|
public boolean isConnected() { |
return con != null && con.isConnected(); |
} |
|
public void addConnectionListener(ConnectionListener listener) { |
AVRSystem.LOG.finest("Added Connection Listener: " + listener); |
connectionListeners.add(listener); |
} |
|
public void removeConnectionListener(ConnectionListener listener) { |
AVRSystem.LOG.finest("Removed Connection Listener: " + listener); |
connectionListeners.remove(listener); |
} |
|
public void connected(ConnectionEvent event) { |
ConnectionListener[] listeners = (ConnectionListener[])connectionListeners.toArray(new ConnectionListener[connectionListeners.size()]); |
for(int i = 0; i < listeners.length; i++) { |
((ConnectionListener)listeners[i]).connected(event); |
} |
} |
|
public void disconnected(ConnectionEvent event) { |
ConnectionListener[] listeners = (ConnectionListener[])connectionListeners.toArray(new ConnectionListener[connectionListeners.size()]); |
for(int i = 0; i < listeners.length; i++) { |
((ConnectionListener)listeners[i]).disconnected(event); |
} |
} |
|
public void connect() throws Exception { |
|
if(!isConnected()) { |
con.connect(); |
|
if(con instanceof SerialConnection) { |
SerialPort serialPort = (SerialPort)con.getConnectionObject(); |
serialPort.notifyOnDataAvailable(true); |
try { |
serialPort.addEventListener(new SerialEventHandler(this)); |
} catch(TooManyListenersException tmle) { |
AVRSystem.LOG.severe(tmle.getMessage()); |
} |
} |
|
in = new AVRInputStream(con.getInputStream()); |
out = con.getOutputStream(); |
|
AVRSystem.LOG.config("Device connected to " + con.toString()); |
|
connected(new ConnectionEvent(con)); |
|
} |
} |
|
public void disconnect() { |
|
if(isConnected()) { |
try { |
con.disconnect(); |
AVRSystem.LOG.config("Device Disconnected"); |
disconnected(new ConnectionEvent(con)); |
con = null; |
} catch(IOException ioe) { |
AVRSystem.LOG.severe(ioe.getMessage()); |
} |
} |
} |
|
public InputStream getInputStream() { |
return in; |
} |
|
public void addDataListener(DataListener dl) { |
if(dataListeners.add(dl)) { |
AVRSystem.LOG.finest("Added Data Listener: " + dl); |
} |
} |
|
public void removeDataListener(DataListener dl) { |
if(dataListeners.remove(dl)) { |
AVRSystem.LOG.finest("Removed Data Listener: " + dl); |
} |
} |
|
protected void handleString(String data) { |
if(data.equals(ACK)) { |
fireACKReceived(); |
} else if(data.equals(NCK)) { |
fireNCKReceived(); |
} else if(data.startsWith(VERSION)) { |
fireVERSIONReceived(data); |
} else { |
StringBuffer builder = new StringBuffer("UNKNOWN PACKET: ("); |
ByteBuffer bytes = ByteBuffer.wrap(data.getBytes()); |
builder.append(bytes.remaining()).append(") "); |
while(bytes.hasRemaining()) { |
builder.append(Integer.toHexString(bytes.get() & 0xFF)).append(' '); |
} |
AVRSystem.LOG.warning(builder.toString()); |
} |
} |
|
protected void handleData(ByteBuffer data) { |
if(data.hasRemaining()) { |
byte dataType = data.get(); |
if(dataType == 0x0B) { |
fireFrameDataReceived(data); |
} else if(dataType == 0x0A) { |
fireTrackingDataReceived(data); |
} else { |
handleString(new String(data.array(), 0, data.limit())); |
} |
} |
} |
|
protected void fireACKReceived() { |
if(responseTask != null) { |
responseTask.cancel(); |
} |
AVRSystem.LOG.info("Received: ACK"); |
DataListener[] listeners = (DataListener[])dataListeners.toArray(new DataListener[dataListeners.size()]); |
for(int i = 0; i < listeners.length; i++) { |
((DataListener)listeners[i]).ack(); |
} |
} |
|
private void fireResponseTimerExpired() { |
AVRSystem.LOG.severe("Response Timer Expired"); |
DataListener[] listeners = (DataListener[])dataListeners.toArray(new DataListener[dataListeners.size()]); |
for(int i = 0; i < listeners.length; i++) { |
((DataListener)listeners[i]).responseTimerExpired(); |
} |
} |
|
protected void fireNCKReceived() { |
if(responseTask != null) { |
responseTask.cancel(); |
} |
AVRSystem.LOG.info("Received: NCK"); |
DataListener[] listeners = (DataListener[])dataListeners.toArray(new DataListener[dataListeners.size()]); |
for(int i = 0; i < listeners.length; i++) { |
((DataListener)listeners[i]).nck(); |
} |
} |
|
protected void fireVERSIONReceived(String version) { |
if(responseTask != null) { |
responseTask.cancel(); |
} |
AVRSystem.LOG.info("Received: " + version); |
DataListener[] listeners = (DataListener[])dataListeners.toArray(new DataListener[dataListeners.size()]); |
for(int i = 0; i < listeners.length; i++) { |
((DataListener)listeners[i]).version(version); |
} |
} |
|
protected void fireFrameDataReceived(ByteBuffer data) { |
|
int position = data.position(); |
StringBuffer buf = new StringBuffer("Received: Frame Dump (" + (data.get(position) & 0xFF) + ")"); |
|
data.position(0); |
|
while(data.hasRemaining()) { |
int b = data.get() & 0xFF; |
buf.append(' ').append(((b & 0xF0) == 0) ? "0" : "").append(Integer.toHexString(b).toUpperCase()); |
} |
|
data.position(position); |
|
AVRSystem.LOG.info(buf.toString()); |
// AVRSystem.LOG.info("Received: Frame Dump (" + (data.get(data.position()) & 0xFF) + ")"); |
DataListener[] listeners = (DataListener[])dataListeners.toArray(new DataListener[dataListeners.size()]); |
for(int i = 0; i < listeners.length; i++) { |
data.mark(); |
((DataListener)listeners[i]).frameData(data); |
data.reset(); |
} |
} |
|
protected void fireTrackingDataReceived(ByteBuffer data) { |
|
int position = data.position(); |
StringBuffer buf = new StringBuffer("Received: Tracking Info (" + (data.get(position) & 0xFF) + ")"); |
|
data.position(0); |
|
while(data.hasRemaining()) { |
int b = data.get() & 0xFF; |
buf.append(' ').append(((b & 0xF0) == 0) ? "0" : "").append(Integer.toHexString(b).toUpperCase()); |
} |
|
data.position(position); |
|
AVRSystem.LOG.info(buf.toString()); |
DataListener[] listeners = (DataListener[])dataListeners.toArray(new DataListener[dataListeners.size()]); |
for(int i = 0; i < listeners.length; i++) { |
data.mark(); |
((DataListener)listeners[i]).trackingData(data); |
data.reset(); |
} |
} |
|
private void sendRequest(byte[] data) throws IOException { |
sendRequest(data, 0, data.length); |
} |
|
private void sendRequest(byte[] data, int off, int len) throws IOException { |
|
responseTask = new ResponseTask(); |
responseTimer.schedule(responseTask, RESPONSE_TIMEOUT); |
|
if(isConnected()) { |
|
out.write(data, off, len); |
out.write((byte)'\n'); |
out.flush(); |
|
StringBuffer builder = new StringBuffer("Sending: "); |
for(int i = off; i < len; i++) { |
builder.append((char)data[i]); |
} |
AVRSystem.LOG.info(builder.toString()); |
} else { |
AVRSystem.LOG.warning("AVRcam not connected."); |
} |
} |
|
public void sendSetRegisters(Map registers) throws IOException { |
ByteBuffer buffer = ByteBuffer.allocate(60); |
buffer.put((byte)'C') |
.put((byte)'R'); |
|
Set entries = registers.entrySet(); |
for(Iterator i = entries.iterator(); i.hasNext();) { |
Map.Entry entry = (Map.Entry)i.next(); |
buffer.put((byte)' ').put(entry.getKey().toString().getBytes()) |
.put((byte)' ').put(entry.getValue().toString().getBytes()); |
} |
|
buffer.put((byte)'\r'); |
buffer.flip(); |
sendRequest(buffer.array(), 0, buffer.remaining()); |
} |
|
public void sendDisableTracking() throws IOException { |
sendRequest(new byte[] { (byte)'D', (byte)'T', (byte)'\r'} ); |
} |
|
public void sendDumpFrame() throws IOException { |
sendRequest(new byte[] { (byte)'D', (byte)'F', (byte)'\r'} ); |
} |
|
public void sendEnableTracking() throws IOException { |
sendRequest(new byte[] { (byte)'E', (byte)'T', (byte)'\r'} ); |
} |
|
public void sendGetVersion() throws IOException { |
sendRequest(new byte[] { (byte)'G', (byte)'V', (byte)'\r'} ); |
} |
|
public void sendPing() throws IOException { |
sendRequest(new byte[] { (byte)'P', (byte)'G', (byte)'\r'} ); |
} |
|
public void sendReset() throws IOException { |
sendRequest(new byte[] { (byte)'R', (byte)'S', (byte)'\r'} ); |
} |
|
public void sendSetColorMap(int[] red, int[] green, int[] blue) throws IOException { |
ByteBuffer buffer = ByteBuffer.allocate(2 + 4 * red.length * 3 + 1); |
buffer.put("SM".getBytes()); |
for(int i = 0; i < red.length; i++) { |
buffer.put((" " + red[i] + "").getBytes()); |
} |
for(int i = 0; i < green.length; i++) { |
buffer.put((" " + green[i] + "").getBytes()); |
} |
for(int i = 0; i < blue.length; i++) { |
buffer.put((" " + blue[i] + "").getBytes()); |
} |
buffer.put("\r".getBytes()); |
buffer.flip(); |
sendRequest(buffer.array(), 0, buffer.remaining()); |
} |
|
public void sendCameraData(ByteBuffer data) throws IOException { |
|
out.write((byte)0x0A); |
out.write(data.array(), 0, data.remaining()); |
out.write((byte)0xFF); |
|
out.flush(); |
|
} |
|
private final static class SerialEventHandler implements SerialPortEventListener { |
|
private Device device; |
|
public SerialEventHandler(Device device) { |
this.device = device; |
} |
|
public void serialEvent(SerialPortEvent spe) { |
if(spe.getEventType() == SerialPortEvent.DATA_AVAILABLE) { |
|
try { |
|
do { |
|
byte[] data = new byte[1024]; |
|
int bytesRead = device.getInputStream().read(data); |
if(bytesRead > 0) { |
// take only bytesRead - 1 to remove the |
// terminating character '\r' or 0xFF |
EventQueue.invokeLater(new GUITask(device, ByteBuffer.wrap(data, 0, bytesRead - 1))); |
} |
|
} while(device.getInputStream().available() > 0); |
|
} catch(IOException ioe) { |
AVRSystem.LOG.severe(ioe.getMessage()); |
device.disconnect(); |
} |
|
} |
} |
|
} |
|
private final static class GUITask implements Runnable { |
private ByteBuffer data; |
private Device device; |
|
public GUITask(Device device, ByteBuffer data) { |
this.device = device; |
this.data = data; |
} |
|
public void run() { |
device.handleData(data); |
} |
|
} |
|
private final class ResponseTask extends TimerTask { |
|
public void run() { |
// make sure we fire the timer expired event |
// using the Event Dispatch Thread. If not, |
// queue this TimerTask in the Event Queue. |
if(EventQueue.isDispatchThread()) { |
fireResponseTimerExpired(); |
} else { |
EventQueue.invokeLater(this); |
} |
} |
|
} |
|
/** |
* Only used for debugging purposes, this will simulate an ACK coming from the device. |
*/ |
public void simulateACK() { |
fireACKReceived(); |
} |
|
/** |
* Only used for debugging purposes, this will simulate an NCK coming from the device. |
*/ |
public void simulateNCK() { |
fireNCKReceived(); |
} |
|
} |