rik/mimis
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Browse Source
This commit is contained in:
Rik Veenboer
2011-02-04 13:01:42 +00:00
parent d1c20dd63c
commit 6ff0592e4f
40 changed files with 0 additions and 5199 deletions
Download Patch File Download Diff File Expand all files Collapse all files

234
java/src/org/wiigee/device/Device.java
View File

@@ -1,234 +0,0 @@
/*
* wiigee - accelerometerbased gesture recognition
* Copyright (C) 2007, 2008, 2009 Benjamin Poppinga
*
* Developed at University of Oldenburg
* Contact: wiigee@benjaminpoppinga.de
*
* This file is part of wiigee.
*
* wiigee 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 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 Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
package org.wiigee.device;
import java.io.IOException;
import java.util.Vector;
import org.wiigee.logic.*;
import org.wiigee.event.*;
import org.wiigee.filter.*;
/**
* Abstract representation of a device with very basic functionalities. This
* class should be derived from, if anybody plans to add a new class of devices,
* like Wiimote or AndroidDevice does. This class mainly consist of filter
* management, recognition control and core event control.
*
* @author Benjamin 'BePo' Poppinga
*/
public class Device {
// Fixed number values.
public static final int MOTION = 0;
// Buttons for action coordination
protected int recognitionbutton;
protected int trainbutton;
protected int closegesturebutton;
// Functional
protected boolean accelerationEnabled;
// Filters, can filter the data stream
protected Vector<Filter> accfilters = new Vector<Filter>();
// Listeners, receive generated events
protected Vector<AccelerationListener> accelerationlistener = new Vector<AccelerationListener>();
protected Vector<ButtonListener> buttonlistener = new Vector<ButtonListener>();
// Processing unit to analyze the data
protected ProcessingUnit processingunit = new TriggeredProcessingUnit();
public Device(boolean autofiltering) {
if (autofiltering) {
this.addAccelerationFilter(new IdleStateFilter());
this.addAccelerationFilter(new MotionDetectFilter(this));
this.addAccelerationFilter(new DirectionalEquivalenceFilter());
}
this.addAccelerationListener(this.processingunit);
this.addButtonListener(this.processingunit);
}
/**
* Adds a Filter for processing the acceleration values.
* @param filter The Filter instance.
*/
public void addAccelerationFilter(Filter filter) {
this.accfilters.add(filter);
}
/**
* Resets all the accfilters, which are resetable.
* Sometimes they have to be resettet if a new gesture starts.
*/
public void resetAccelerationFilters() {
for (int i = 0; i < this.accfilters.size(); i++) {
this.accfilters.elementAt(i).reset();
}
}
/**
* Adds an AccelerationListener to the Device. Everytime an acceleration
* on the Device is performed the AccelerationListener would receive
* an event of this action.
*
* @param listener The Listener.
*/
public void addAccelerationListener(AccelerationListener listener) {
this.accelerationlistener.add(listener);
}
/**
* Adds a ButtonListener to the Device. Everytime a Button has been
* pressed or released, the Listener would be notified about this via
* the corresponding Events.
*
* @param listener The Listener.
*/
public void addButtonListener(ButtonListener listener) {
this.buttonlistener.add(listener);
}
/**
* Adds a GestureListener to the Device. Everytime a gesture
* is performed the GestureListener would receive an event of
* this gesture.
*
* @param listener The Listener.
*/
public void addGestureListener(GestureListener listener) {
this.processingunit.addGestureListener(listener);
}
public int getRecognitionButton() {
return this.recognitionbutton;
}
public void setRecognitionButton(int b) {
this.recognitionbutton = b;
}
public int getTrainButton() {
return this.trainbutton;
}
public void setTrainButton(int b) {
this.trainbutton = b;
}
public int getCloseGestureButton() {
return this.closegesturebutton;
}
public void setCloseGestureButton(int b) {
this.closegesturebutton = b;
}
public ProcessingUnit getProcessingUnit() {
return this.processingunit;
}
public void setAccelerationEnabled(boolean enabled) throws IOException {
this.accelerationEnabled = enabled;
}
public void loadGesture(String filename) {
this.processingunit.loadGesture(filename);
}
public void saveGesture(int id, String filename) {
this.processingunit.saveGesture(id, filename);
}
// ###### Event-Methoden
/** Fires an acceleration event.
* @param vector Consists of three values:
* acceleration on X, Y and Z axis.
*/
public void fireAccelerationEvent(double[] vector) {
for (int i = 0; i < this.accfilters.size(); i++) {
vector = this.accfilters.get(i).filter(vector);
// cannot return here if null, because of time-dependent accfilters
}
// don't need to create an event if filtered away
if (vector != null) {
// calculate the absolute value for the accelerationevent
double absvalue = Math.sqrt((vector[0] * vector[0]) +
(vector[1] * vector[1]) + (vector[2] * vector[2]));
AccelerationEvent w = new AccelerationEvent(this,
vector[0], vector[1], vector[2], absvalue);
for (int i = 0; i < this.accelerationlistener.size(); i++) {
this.accelerationlistener.get(i).accelerationReceived(w);
}
}
} // fireaccelerationevent
/** Fires a button pressed event.
* @param button
* Integer value of the pressed button.
*/
public void fireButtonPressedEvent(int button) {
ButtonPressedEvent w = new ButtonPressedEvent(this, button);
for (int i = 0; i < this.buttonlistener.size(); i++) {
this.buttonlistener.get(i).buttonPressReceived(w);
}
if (w.isRecognitionInitEvent() || w.isTrainInitEvent()) {
this.resetAccelerationFilters();
}
}
/** Fires a button released event.
*/
public void fireButtonReleasedEvent(int button) {
ButtonReleasedEvent w = new ButtonReleasedEvent(this, button);
for (int i = 0; i < this.buttonlistener.size(); i++) {
this.buttonlistener.get(i).buttonReleaseReceived(w);
}
}
/**
* Fires a motion start event.
*/
public void fireMotionStartEvent() {
MotionStartEvent w = new MotionStartEvent(this);
for (int i = 0; i < this.accelerationlistener.size(); i++) {
this.accelerationlistener.get(i).motionStartReceived(w);
}
}
/**
* Fires a motion stop event.
*/
public void fireMotionStopEvent() {
MotionStopEvent w = new MotionStopEvent(this);
for (int i = 0; i < this.accelerationlistener.size(); i++) {
this.accelerationlistener.get(i).motionStopReceived(w);
}
}
}

602
java/src/org/wiigee/device/Wiimote.java
View File

@@ -1,602 +0,0 @@
/*
* wiigee - accelerometerbased gesture recognition
* Copyright (C) 2007, 2008, 2009 Benjamin Poppinga
*
* Developed at University of Oldenburg
* Contact: wiigee@benjaminpoppinga.de
*
* This file is part of wiigee.
*
* wiigee 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 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 Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
package org.wiigee.device;
import java.io.IOException;
import java.util.Random;
import java.util.Vector;
import javax.bluetooth.L2CAPConnection;
import javax.microedition.io.Connector;
import org.wiigee.event.*;
import org.wiigee.filter.Filter;
import org.wiigee.util.Log;
/**
* This class represents the basic functions of the wiimote.
* If you want your wiimote to e.g. vibrate you'll do this here.
*
* @author Benjamin 'BePo' Poppinga
*/
public class Wiimote extends Device {
// Fixed number values.
public static final int BUTTON_2 = 0x0001;
public static final int BUTTON_1 = 0x0002;
public static final int BUTTON_B = 0x0004;
public static final int BUTTON_A = 0x0008;
public static final int BUTTON_MINUS = 0x0010;
public static final int BUTTON_HOME = 0x0080;
public static final int BUTTON_LEFT = 0x0100;
public static final int BUTTON_RIGHT = 0x0200;
public static final int BUTTON_DOWN = 0x0400;
public static final int BUTTON_UP = 0x0800;
public static final int BUTTON_PLUS = 0x1000;
// Reports
public static final byte CMD_SET_REPORT = 0x52;
// IR Modes
public static final byte IR_MODE_STANDARD = 0x01;
public static final byte IR_MODE_EXTENDED = 0x03;
// Modes / Channels
public static final byte MODE_BUTTONS = 0x30;
public static final byte MODE_BUTTONS_ACCELERATION = 0x31;
public static final byte MODE_BUTTONS_ACCELERATION_INFRARED = 0x33;
// Bluetooth-adress as string representation
private String btaddress;
// LED encoded as byte
byte ledencoding;
// Filters, can filter the data stream
protected Vector<Filter> rotfilters = new Vector<Filter>();
// control connection, send commands to wiimote
private L2CAPConnection controlCon;
// receive connection, receive answers from wiimote
private L2CAPConnection receiveCon;
// Listeners, receive generated events
protected Vector<InfraredListener> infraredlistener = new Vector<InfraredListener>();
protected Vector<RotationListener> rotationListener = new Vector<RotationListener>();
// keep track of the orientation
private double pitch = 0.0;
private double roll = 0.0;
private double yaw = 0.0;
// Functional
private boolean vibrating;
private boolean calibrated;
private boolean infraredEnabled;
private WiimoteStreamer wms;
private boolean wiiMotionPlusEnabled;
/**
* Creates a new wiimote-device with a specific bluetooth mac-adress.
*
* @param btaddress
* String representation of the mac-adress e.g. 00191D68B57C.
* @param autofiltering
* If set the wiimote would automatically add the IdleStateFilter.
* @param autoconnect
* If set the wiimote would automatically be connected.
*/
public Wiimote(String btaddress, boolean autofiltering, boolean autoconnect) throws IOException {
super(autofiltering);
this.btaddress = this.removeChar(btaddress, ':');
this.vibrating = false;
this.setCloseGestureButton(Wiimote.BUTTON_HOME);
this.setRecognitionButton(Wiimote.BUTTON_B);
this.setTrainButton(Wiimote.BUTTON_A);
// automatic connect enabled
if (autoconnect) {
this.connect();
this.calibrateAccelerometer();
this.streamData(true);
this.setLED(1);
this.setAccelerationEnabled(true);
}
}
/**
* Creates the two needed connections to send and receive commands
* to and from the wiimote-device.
*
*/
public void connect() throws IOException {
this.controlCon = (L2CAPConnection) Connector.open("btl2cap://" +
this.btaddress + ":11;authenticate=false;encrypt=false;master=false",
Connector.WRITE); // 11
this.receiveCon = (L2CAPConnection) Connector.open("btl2cap://" +
this.btaddress + ":13;authenticate=false;encrypt=false;master=false",
Connector.READ); // 13
}
/**
* Disconnects the wiimote and closes the two connections.
*/
public void disconnect() {
this.vibrating = false;
try {
this.controlCon.close();
this.receiveCon.close();
Log.write("Disconnected wiimote.");
} catch (Exception e) {
Log.write("Failure during disconnect of wiimote.");
}
}
/**
* @return
* Receiving data connection
*/
public L2CAPConnection getReceiveConnection() {
return this.receiveCon;
}
/**
* This method makes the Wiimote-Class reacting to incoming data.
* For just controlling and sending commands to the wiimote
* (vibration, LEDs, ...) it's not necessary to call this method.
*
* @param value
* true, if the class should react to incoming data.
* false, if you only want to send commands to wiimote and
* only the control-connection is used.
*/
public void streamData(boolean value) {
if (value == true) {
if (this.wms == null) {
this.wms = new WiimoteStreamer(this);
}
wms.start();
} else if (this.wms != null) {
wms.stopThread();
}
}
/**
* The added Listener will be notified about detected infrated
* events.
*
* @param listener The Listener to be added.
*/
public void addInfraredListener(InfraredListener listener) {
this.infraredlistener.add(listener);
}
/**
* The added Listener will be notified about detected orientation
* changes.
*
* @param listener The Listener to be added.
*/
public void addRotationListener(RotationListener listener) {
this.rotationListener.add(listener);
}
/**
* Adds a filter to process the rotation speed data of the
* wiimote with an attached Wii Motion Plus.
*
* @param filter The Filter to be added.
*/
public void addRotationFilter(Filter filter) {
this.rotfilters.add(filter);
}
/**
* Resets all filters which are applied to the rotation data
* from the Wii Motion Plus. Also resets _all_ determined orientation
* angles, which should be extended with a consideration of other
* external datas - maybe irda events.
*/
public void resetRotationFilters() {
this.yaw = 0.0;
this.pitch = 0.0;
this.roll = 0.0;
for (int i = 0; i < this.rotfilters.size(); i++) {
this.rotfilters.elementAt(i).reset();
}
}
/**
* Write data to a register inside of the wiimote.
*
* @param offset The memory offset, 3 bytes.
* @param data The data to be written, max. 16 bytes.
* @throws IOException
*/
public void writeRegister(byte[] offset, byte[] data) throws IOException {
byte[] raw = new byte[23];
raw[0] = CMD_SET_REPORT;
raw[1] = 0x16; // Write channel
raw[2] = 0x04; // Register
for (int i = 0; i < offset.length; i++) {
raw[3 + i] = offset[i];
}
raw[6] = (byte) data.length;
for (int i = 0; i < data.length; i++) {
raw[7 + i] = data[i];
}
this.sendRaw(raw);
}
/**
* Makes the Wiimote respond the data of an register. The wiimotestreamer
* doesn't react to the reponse yet.
*
* @param offset The memory offset.
* @param size The size which has to be read out.
* @throws IOException
*/
public void readRegister(byte[] offset, byte[] size) throws IOException {
byte[] raw = new byte[8];
raw[0] = CMD_SET_REPORT;
raw[1] = 0x17; // Read channel
raw[2] = 0x04; // Register
for (int i = 0; i < offset.length; i++) {
raw[3 + i] = offset[i];
}
for (int i = 0; i < size.length; i++) {
raw[6 + i] = size[i];
}
this.sendRaw(raw);
}
/**
* Reads data out of the EEPROM of the wiimote.
* At the moment this method is only used to read out the
* calibration data, so the wiimotestreamer doesn't react for
* every answer on this request.
*
* @param offset The memory offset.
* @param size The size.
* @throws IOException
*/
public void readEEPROM(byte[] offset, byte[] size) throws IOException {
byte[] raw = new byte[8];
raw[0] = CMD_SET_REPORT;
raw[1] = 0x17; // Read channel
raw[2] = 0x00; // EEPROM
for (int i = 0; i < offset.length; i++) {
raw[3 + i] = offset[i];
}
for (int i = 0; i < size.length; i++) {
raw[6 + i] = size[i];
}
this.sendRaw(raw);
}
/**
* Sends pure hexdata to the wiimote. If you want your wiimote
* to vibrate use sendRaw(new byte[] {0x52, 0x13, 0x01}). For other raw-commands use
* the specific wiki-sites around the web (wiili.org, wiibrew.org, ...)
* @param raw
* byte representation of an command
*/
public void sendRaw(byte[] raw) throws IOException {
if (this.controlCon != null) {
this.controlCon.send(raw);
try {
Thread.sleep(100l);
} catch (InterruptedException e) {
System.out.println("sendRaw() interrupted");
}
}
}
/**
* Enables one or more LEDs, where the value could be between 0 and 8.
* If value=1 only the left LED would light up, for value=2 the second
* led would light up, for value=3 the first and second led would light up,
* and so on...
*
* @param value Between 0 and 8, indicating which LEDs should light up
* @throws IOException
*/
public void setLED(int value) throws IOException {
if (value < 16 && value > 0) {
byte tmp = (byte) value;
this.ledencoding = (byte) (tmp << 4);
this.sendRaw(new byte[]{CMD_SET_REPORT, 0x11, this.ledencoding});
} else {
// Random LED change :)
this.setLED(new Random().nextInt(16));
}
}
/**
* Updates the report channel according to the choosen
* functions that are enabled (acceleration, irda, ...).
*
*/
private void updateReportChannel() throws IOException {
if(!accelerationEnabled
&& !wiiMotionPlusEnabled
&& !infraredEnabled) {
this.sendRaw(new byte[]{CMD_SET_REPORT, 0x12, 0x00, 0x30});
}
else if(accelerationEnabled
&& !wiiMotionPlusEnabled
&& !infraredEnabled) {
this.sendRaw(new byte[]{CMD_SET_REPORT, 0x12, 0x04, 0x31});
}
else if(!accelerationEnabled
&& wiiMotionPlusEnabled
&& !infraredEnabled) {
this.sendRaw(new byte[]{CMD_SET_REPORT, 0x12, 0x00, 0x32});
}
else if(accelerationEnabled
&& wiiMotionPlusEnabled
&& !infraredEnabled) {
this.sendRaw(new byte[]{CMD_SET_REPORT, 0x12, 0x04, 0x35});
}
else if(accelerationEnabled
&& !wiiMotionPlusEnabled
&& infraredEnabled) {
this.sendRaw(new byte[]{CMD_SET_REPORT, 0x12, 0x04, 0x33});
}
else if(accelerationEnabled
&& wiiMotionPlusEnabled
&& infraredEnabled) {
this.sendRaw(new byte[]{CMD_SET_REPORT, 0x12, 0x04, 0x37});
}
else {
// default channel - fallback to button only.
Log.write("Invalid Value Configuration: Fallback to Buttons only.");
this.sendRaw(new byte[]{CMD_SET_REPORT, 0x12, 0x00, 0x30});
}
}
/**
* Initializes the calibration of the accerlerometer. This is done once
* per each controller in program lifetime.
*
* @throws IOException
*/
private void calibrateAccelerometer() throws IOException {
this.readEEPROM(new byte[]{0x00, 0x00, 0x20}, new byte[]{0x00, 0x07});
this.calibrated = true;
}
/**
* Activates the acceleration sensor. You have to call the
* streamData(true) method to react to this acceleration data.
* Otherwise the wiimote would send data the whole time and
* nothing else would happen.
*
*/
@Override
public void setAccelerationEnabled(boolean enabled) throws IOException {
super.setAccelerationEnabled(enabled);
if(enabled) {
Log.write("Enabling ACCELEROMETER...");
this.accelerationEnabled = true;
if (!this.calibrated) {
this.calibrateAccelerometer();
}
} else {
Log.write("Disabling ACCELEROMETER...");
this.accelerationEnabled = false;
}
// change channel dynamically
this.updateReportChannel();
}
/**
* Enables or disables the infrared camera of the wiimote with
* the default values.
*
* @param e Should the Infrared Camera be enabled.
* @throws IOException In case of a connection error.
*/
public void setInfraredCameraEnabled(boolean enabled) throws IOException {
this.setInfraredCameraEnabled(enabled, Wiimote.IR_MODE_STANDARD);
}
/**
* Enables the infrared camera in front of the wiimote to track
* IR sources in the field of view of the camera. This could be used
* to a lot of amazing stuff. Using this Mode could slow down the
* recognition of acceleration gestures during the increased data
* size transmitted.
*
* @param e Should the Infrared Camera be enabled.
* @param infraredMode The choosen Infrared Camera Mode.
* @throws IOException In case of a connection error.
*
*/
public void setInfraredCameraEnabled(boolean enabled, byte infraredMode) throws IOException {
if(enabled) {
Log.write("Enabling INFRARED CAMERA...");
this.infraredEnabled = true;
//write 0x04 to output 0x13
this.sendRaw(new byte[]{CMD_SET_REPORT, 0x13, 0x04});
// write 0x04 to output 0x1a
this.sendRaw(new byte[]{CMD_SET_REPORT, 0x1a, 0x04});
// write 0x08 to reguster 0xb00030
this.writeRegister(new byte[]{(byte) 0xb0, 0x00, 0x30}, new byte[]{0x08});
// write sensivity block 1 to register 0xb00000
this.writeRegister(new byte[]{(byte) 0xb0, 0x00, 0x00}, new byte[]{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, (byte) 0x90, 0x00, (byte) 0x41});
// write sensivity block 2 to register 0xb0001a
this.writeRegister(new byte[]{(byte) 0xb0, 0x00, (byte) 0x1a}, new byte[]{0x40, 0x00});
// write ir-mode to register 0xb00033
this.writeRegister(new byte[]{(byte) 0xb0, 0x00, 0x33}, new byte[]{infraredMode});
} else {
Log.write("Disabling INFRARED CAMERA...");
this.infraredEnabled = false;
}
// change channel dynamically
this.updateReportChannel();
}
/**
* To enable the Wii Motion Plus extension. The wiimote will further get
* every other information, like acceleration, infrared camera (loss of precision)
* and button presses.
*
* @throws java.io.IOException
*/
public void setWiiMotionPlusEnabled(boolean enabled) throws IOException {
if(enabled) {
Log.write("Enabling WII MOTION PLUS..");
this.wiiMotionPlusEnabled = true;
// write 0x04 to 0x04a600fe to get wii m+ data within extension reports
this.writeRegister(new byte[]{(byte) 0xa6, 0x00, (byte) 0xfe}, new byte[]{0x04});
} else {
Log.write("Disabling WII MOTION PLUS..");
this.wiiMotionPlusEnabled = false;
}
// change channel dynamically
this.updateReportChannel();
}
/**
* With this method you gain access over the vibrate function of
* the wiimote. You got to try which time in milliseconds would
* fit your requirements.
*
* @param milliseconds Time the wiimote would approx. vibrate.
*/
public void vibrateForTime(long milliseconds) throws IOException {
try {
if (!vibrating) {
this.vibrating = true;
byte tmp = (byte) (this.ledencoding | 0x01);
this.sendRaw(new byte[]{CMD_SET_REPORT, 0x11, tmp});
Thread.sleep(milliseconds);
this.sendRaw(new byte[]{CMD_SET_REPORT, 0x11, this.ledencoding});
this.vibrating = false;
}
} catch (InterruptedException e) {
System.out.println("WiiMoteThread interrupted.");
}
}
public double getPitch() {
return this.pitch;
}
public double getYaw() {
return this.yaw;
}
public double getRoll() {
return this.roll;
}
/**
* Fires a infrared event, containig coordinate pairs (x,y) and a
* size of the detected IR spot.
*
* @param coordinates X and Y display coordinates.
* @param size The size of the spot.
*/
public void fireInfraredEvent(int[][] coordinates, int[] size) {
InfraredEvent w = new InfraredEvent(this, coordinates, size);
for (int i = 0; i < this.infraredlistener.size(); i++) {
this.infraredlistener.get(i).infraredReceived(w);
}
}
/**
* Fires the current relative orientation of the Wiimote to
* all RotationListeners.
*
* @param yaw Orientation around Z axis.
* @param roll Orientation around Y axis.
* @param pitch Orientation around X axis.
*/
public void fireRotationEvent(double pitch, double roll, double yaw) {
this.pitch = pitch;
this.roll = roll;
this.yaw = yaw;
RotationEvent w = new RotationEvent(this, pitch, roll, yaw);
for (int i = 0; i < this.rotationListener.size(); i++) {
this.rotationListener.elementAt(i).rotationReceived(w);
}
}
/**
* If a Wii Motion Plus is attached and activated properly this
* event could be fired within every change of orientation of the
* device. The orientation is not used to do gesture recognition,
* yet.
*
* @param vector The rotational speed vector, containing:
* phi - Rotational speed of x axis (pitch)
* theta - Rotational speed of y axis (roll)
* psi - Rotational speed of z axis (yaw)
*/
public void fireRotationSpeedEvent(double[] vector) {
for (int i = 0; i < this.rotfilters.size(); i++) {
vector = this.rotfilters.get(i).filter(vector);
// cannot return here if null, because of time-dependent filters
}
if (vector != null) {
RotationSpeedEvent w = new RotationSpeedEvent(this, vector[0], vector[1], vector[2]);
for (int i = 0; i < this.rotationListener.size(); i++) {
this.rotationListener.elementAt(i).rotationSpeedReceived(w);
}
// calculate new orientation with integration
// do not store new global values here, since they
// need regular updates only depended on acceleration values.
double tyaw = this.yaw + vector[0] * 0.01;
double troll = this.roll + vector[1] * 0.01;
double tpitch = this.pitch + vector[2] * 0.01;
this.fireRotationEvent(tpitch, troll, tyaw);
}
}
// ###### Hilfsmethoden
// TODO
private String removeChar(String s, char c) {
String r = "";
for (int i = 0; i < s.length(); i++) {
if (s.charAt(i) != c) {
r += s.charAt(i);
}
}
return r;
}
}

400
java/src/org/wiigee/device/WiimoteStreamer.java
View File

@@ -1,400 +0,0 @@
/*
* wiigee - accelerometerbased gesture recognition
* Copyright (C) 2007, 2008, 2009 Benjamin Poppinga
*
* Developed at University of Oldenburg
* Contact: wiigee@benjaminpoppinga.de
*
* This file is part of wiigee.
*
* wiigee 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 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 Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
package org.wiigee.device;
import java.io.IOException;
import java.util.EventObject;
import java.util.Vector;
import javax.bluetooth.L2CAPConnection;
import org.wiigee.util.Log;
/**
* This class listens to data sended by the wiimote and generates specific
* events for acceleration, buttonpress, ...
*
* @author Benjamin 'BePo' Poppinga
*
*/
public class WiimoteStreamer extends Thread {
private boolean running;
private int buttonstate;
private double x0, x1, y0, y1, z0, z1;
private double psi0, theta0, phi0;
private boolean wmpcalibrated;
private int calibrationcounter;
private Vector<double[]> calibrationsequence;
private Wiimote wiimote;
private L2CAPConnection receiveCon;
private EventObject lastevent;
protected WiimoteStreamer(Wiimote wiimote) {
this.wiimote = wiimote;
this.receiveCon = wiimote.getReceiveConnection();
this.buttonstate = 0;
Log.write("WiimoteStreamer initialized...");
}
@Override
public void run() {
Log.write("WiimoteStreamer running...");
this.running = true;
this.calibrationcounter = 0;
this.calibrationsequence = new Vector<double[]>();
try {
while (running) {
// connection has data and we're ready.
byte[] b = this.getRaw(); // blocks application
// Log.write("");
// debug output
/* for(int i=0; i<b.length; i++) {
* System.out.print((int)b[i]&0xFF); if(i==input.length-1) {
* System.out.println(""); } else { System.out.print(":"); } }
*/
if((b[1] & 0xFF) == 0x31) {
this.handleButtonData(new byte[] { b[2], b[3] });
this.handleAccelerationData(new byte[] { b[4], b[5], b[6] });
//Log.write("0x31: Button + Acc");
}
else if ((b[1] & 0xFF) == 0x33) {
this.handleButtonData(new byte[] { b[2], b[3] });
this.handleAccelerationData(new byte[]{b[4], b[5], b[6]});
this.handleInfraredData(new byte[]{b[7], b[8], b[9],
b[10], b[11], b[12],
b[13], b[14], b[15],
b[16], b[17], b[18]});
//Log.write("0x33: Button + Acc + Irda");
}
else if ((b[1] & 0xFF) == 0x37) {
this.handleButtonData(new byte[] { b[2], b[3] });
this.handleAccelerationData(new byte[]{b[4], b[5], b[6]});
this.handleInfraredData(
new byte[]{b[7], b[8], b[9], b[10], b[11], b[12],
b[13], b[14], b[15], b[16]});
this.handleWiiMotionPlusData(
new byte[]{b[17], b[18], b[19], b[20], b[21], b[22]});
//Log.write("0x37: Button + Acc + Ext");
}
else if ((b[1] & 0xFF) == 0x21) {
this.handleButtonData(new byte[] { b[2], b[3] });
// calibration data
if (((b[5] & 0xFF) == 0x00) && ((b[6] & 0xFF) == 0x20)) {
this.handleCalibrationData(
new byte[]{b[7], b[8], b[9], b[11], b[12], b[13]});
// Log.write("0x21: Calibration result");
} else {
this.handleRawDataAnswer(
new byte[]{b[5], b[6]},
new byte[]{b[7], b[8], b[9], b[10], b[11], b[12],
b[13], b[14], b[15], b[16], b[17], b[18], b[19], b[20],
b[21], b[22]
});
// Log.write("0x21: Raw data answer");
}
}
else if ((b[1] & 0xFF) == (byte) 0x3d) {
Log.write("0x3D: Ext only");
}
else {
Log.write("Unknown data retrieved.");
this.printBytes(b);
}
} // while(running)
} catch (IOException e) {
Log.write("Streamer: Connection to Wiimote lost.");
this.running = false;
}
}
/**
* Handles calibration Data
*
* @param data An array of bytes, containing the data.
*/
private void handleCalibrationData(byte[] data) {
this.x0 = data[0] & 0xFF;
this.y0 = data[1] & 0xFF;
this.z0 = data[2] & 0xFF;
this.x1 = data[3] & 0xFF;
this.y1 = data[4] & 0xFF;
this.z1 = data[5] & 0xFF;
Log.write("Autocalibration successful!");
}
/**
* Handles a raw data answer: It just prints it out.
*
* @param offset The offset of the maybe read data.
* @param data The data itself.
*/
private void handleRawDataAnswer(byte[] offset, byte[] data) {
String out = "";
String[] o = this.byte2hex(offset);
String[] d = this.byte2hex(data);
out += "READ " + o[0] + "" + o[1] + ": ";
for (int i = 0; i < d.length; i++) {
out += d[i] + " ";
}
Log.write(out);
}
/**
* Handles the Wii Motion Plus data to generate orientation
* events on the Wiimote.
*
* @param data The data containing the raw rotation speeds.
*/
private void handleWiiMotionPlusData(byte[] data) {
// fixed values until calibration procedure is known
//int psi0 = 8265;
//int theta0 = 7963;
//int phi0 = 7923;
//this.printBytes(new byte[]{ data[3], data[4], data[5]});
int psiL = (data[0] & 0xFF);
int thetaL = (data[1] & 0xFF);
int phiL = (data[2] & 0xFF);
// cut two lower bits of UPPER values, shift right
int psiU = ((data[3] & 0xFC) << 6);
int thetaU = ((data[4] & 0xFC) << 6);
int phiU = ((data[5] & 0xFC) << 6);
// add the two values
int psiRAW = psiU + psiL;
int thetaRAW = thetaU + thetaL;
int phiRAW = phiU + phiL;
// average of 50 samples for calibration
if (!this.wmpcalibrated) {
if (this.calibrationcounter++ < 50) {
this.calibrationsequence.add(new double[]{psiRAW, thetaRAW, phiRAW});
} else {
this.calibrateWiiMotionPlus();
}
} else { // is calibrated
// calculate degrees per second movement
double psi = (double) (psiRAW - psi0) / 20.0;
double theta = (double) (thetaRAW - theta0) / 20.0;
double phi = (double) (phiRAW - phi0) / 20.0;
this.wiimote.fireRotationSpeedEvent(new double[]{-psi, -theta, -phi});
}
}
/**
* Build an average of the presaved rotation sequences to have a reference
* value for 'not rotating'. This is a kind of calibration until the original
* calibration procedure for the wii motion plus is known.
*/
private void calibrateWiiMotionPlus() {
for (int i = 0; i < this.calibrationsequence.size(); i++) {
this.psi0 += this.calibrationsequence.elementAt(i)[0];
this.theta0 += this.calibrationsequence.elementAt(i)[1];
this.phi0 += this.calibrationsequence.elementAt(i)[2];
}
this.psi0 /= this.calibrationsequence.size();
this.theta0 /= this.calibrationsequence.size();
this.phi0 /= this.calibrationsequence.size();
this.wmpcalibrated = true;
Log.write("Wii Motion Plus calibrated manually!");
}
/**
* Handles the raw infrared data, containing the spot positions
* and the intensities of the spots. At the moment only the
* 12 Byte irda raw data is supported.
*
* @param data 12 Bytes of raw irda data.
*/
private void handleInfraredData(byte[] data) {
int[][] coordinates = new int[4][2];
int[] size = new int[] { 1, 1, 1, 1 };
int j = 0;
// normal mode
if(data.length == 10) {
for(int i=0; i<10; i+=5) { // for each IR fife-byte segment
int tailX1 = data[i] & 0xFF;
int tailY1 = data[i+1] & 0xFF;
int preY1 = (data[i+2] & 0xC0) << 2;
int preX1 = (data[i+2] & 0x30) << 4;
int preY2 = (data[i+2] & 0x0C) << 6;
int preX2 = (data[i+2] & 0x03) << 8;
int tailX2 = data[i+3] & 0xFF;
int tailY2 = data[i+4] & 0xFF;
coordinates[j][0] = tailX1 + preX1;
coordinates[j][1] = tailY1 + preY1;
coordinates[j+1][0] = tailX2 + preX2;
coordinates[j+1][1] = tailY2 + preY2;
j+=2;
}
} else if (data.length == 12) { // extended mode
for (int i = 0; i < 12; i += 3) { // for each IR byte
// triple
int tailX = data[i] & 0xFF;
int tailY = data[i + 1] & 0xFF;
int preY = (data[i + 2] & 0xC0) << 2;
int preX = (data[i + 2] & 0x30) << 4;
coordinates[j][0] = tailX + preX;
coordinates[j][1] = tailY + preY;
size[j++] = (data[i + 2] & 0x0F);
}
}
this.wiimote.fireInfraredEvent(coordinates, size);
}
/**
* Handles the retrieved acceleration data and fires
* corresponding Acceleration Event on the Device.
*
* @param data
*/
private void handleAccelerationData(byte[] data) {
// convert to int.
int xraw = (data[0] & 0xFF);
int yraw = (data[1] & 0xFF);
int zraw = (data[2] & 0xFF);
// calculate acceleration with calibration data.
double x = (double) (xraw - x0) / (double) (x1 - x0);
double y = (double) (yraw - y0) / (double) (y1 - y0);
double z = (double) (zraw - z0) / (double) (z1 - z0);
this.wiimote.fireAccelerationEvent(new double[]{x, y, z});
}
/**
* Decodes the two bytes with the button press and release bits.
*
* @param a First button byte.
* @param b Second button byte.
*/
private void handleButtonData(byte[] data) {
byte first = (byte)(data[0] & 0xFF);
byte second = (byte)(data[1] & 0xFF);
int newbuttons = (first<<8) + second;
int delta = this.buttonstate ^ newbuttons; // XOR
int shift = 0x0001;
while(shift<0x1000) {
if(shift!=0x0020 && shift!=0x0040) { // reserved bytes
if((delta&shift)==shift) { // change detected
if((newbuttons&shift)==shift) { // press detected
this.wiimote.fireButtonPressedEvent(shift);
} else { // release detected
this.wiimote.fireButtonReleasedEvent(shift);
}
}
}
shift<<=1;
}
this.buttonstate = newbuttons;
}
/**
* Gets 23 bytes out of the Receive Connection Stream.
*
* @return The 23 retrieved bytes.
* @throws java.io.IOException
*/
private byte[] getRaw() throws IOException {
byte[] b = new byte[23];
this.receiveCon.receive(b);
return b;
}
/**
* Stops this thread.
*/
protected void stopThread() {
this.running = false;
}
/**
* true if thread is running.
* @return true if thread is running, false otherwise.
*/
protected boolean isRunning() {
return this.running;
}
/**
* Prints a byte stream as hex string.
* @param b
*/
private void printBytes(byte[] b) {
String out = "";
String[] s = this.byte2hex(b);
for (int i = 0; i < s.length; i++) {
out += " " + s[i];
}
Log.write(out);
}
/**
* Converts a byte array to a string array.
*
* @param b
* @return
*/
private String[] byte2hex(byte[] b) {
String[] out = new String[b.length];
String stmp = "";
for (int n = 0; n < b.length; n++) {
stmp = (java.lang.Integer.toHexString(b[n] & 0XFF));
if (stmp.length() == 1) {
out[n] = ("0" + stmp).toUpperCase();
} else {
out[n] = stmp.toUpperCase();
}
}
return out;
}
}