Android IOIO Wii Motion Plus – Gyroscopes

First of all, the more I work with Android, the more I love it … ! It’s so cool to have both Linux and Java and everything is so open and hackable … !

Wii Motion Plus connected to the IOIO board and then to the Android phone

Today I took some time to set up wireless debugging, in order to be able to see the logs at the same time as the phone is connected through USB to the IOIO board. This was not a big issue for very simple programs, but it can quickly become veeery annoying !

As mentioned on this forum topic having a SSHd server running on the Android device and connecting through WiFi to it from the dev PC is a neat solution.

The best part is that I didn’t have to spend too much time following this kind of complex tutorial. Here’s my approach:

1. root the phone (was already like this, and lucky me I had bought it like this on eBay)
2. BUSYBOX – can be installed from the Android market for free
3. Install QuickSSHd (based on dropbear) from Android market for 0.99£ (I know, some people prefer software that is free like in free beer, and I think there are actually some other ads based versions out there, but I couldn’t be bothered)
4. and that’s it: you can uyse Putty or WinSCP to connect to your phone and transfer files

Not only this allows me to do “logcat” remotely on the phone while it’s connected to the IOIO board, but I can also SCP the “.adk” files and install them manually, allowing me to basically NEVER disconnect it from the IOIO board.

This is AWESOME !

Now back to the main subject of the post…

I basically want to transform the phone into a proper IMU, but it lacks gyroscopes. It already has accelerometers and compasses, but gyros are vital if we want to have any accuracy…

So what better solution than try to attach a Wii Motion Plus (containing 3 nice gyros, all conveniently exposed through a I2C interface) … ? The idea is to do something similar to this Arduino connection, but with the smart phone, through the IOIO board.

Here are the details of the wiring, basically the same thing as for the Wii Nunchuck, the differences are in the code.

Wii Motion Plus - details of the wiring to the IOIO board

 

And finally, the Java / Android code:

IOIOWiiMotionPlus.java
-------------------------------------------------------
package com.trandi.wii;

import ioio.lib.api.TwiMaster;
import ioio.lib.api.exception.ConnectionLostException;
import android.util.Log;

public class IOIOWiiMotionPlus {

private TwiMaster _twi;
// 7bit addressing
private static final int WII_WMP_ADDR_INIT = 0x53;
private static final int WII_WMP_ADDR = 0x52;
private final byte[] _dataBuffRaw = new byte[6];// array to store nunchuck data
private final int[] _dataBuff = new int[6];
private int _gyroX0, _gyroY0, _gyroZ0; //calibration zeroes
private int _gyroX, _gyroY, _gyroZ;

public boolean init(TwiMaster twi) throws ConnectionLostException, InterruptedException {
_twi = twi;

if(!initGyros()) {
Log.w(Util.LOG_TAG, "Can't INTIALIZE Gyros. This might be normal if they are already initialized and the WMP hasn't been turned off.");
}
if(! calibrateGyrosZeroes()) {
Log.e(Util.LOG_TAG, "Can't CALIBRATE Gyros");
return false;
}
return true;
}

private boolean initGyros() throws ConnectionLostException, InterruptedException {
// We don't care about what the WMP returs, but we need to provide a buffer to avoid a
// java.lang.NullPointerException
// E/WII     ( 1164):      at java.lang.System.arraycopy(Native Method)
// E/WII     ( 1164):      at ioio.lib.impl.TwiMasterImpl.dataReceived(TwiMasterImpl.java:141)
boolean res = _twi.writeRead(WII_WMP_ADDR_INIT, false, new byte[]{(byte)0xFE, 0x04}, 2, _dataBuffRaw, 0);
Thread.sleep(10);
return res;
}

public boolean readData() throws ConnectionLostException, InterruptedException {
// Send a request for data and read raw data
if( _twi.writeRead(WII_WMP_ADDR, false, new byte[]{0x00}, 1, _dataBuffRaw, _dataBuffRaw.length)){
for(int i=0; i<_dataBuffRaw.length; i++) _dataBuff[i] = Util.fixJavaByte(_dataBuffRaw[i]);

//see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus for info on what each byte represents
_gyroZ = ((_dataBuff[3] >> 2) << 8) + _dataBuff[0] - _gyroZ0;
_gyroX = ((_dataBuff[4] >> 2) << 8) + _dataBuff[1] - _gyroX0;
_gyroY = ((_dataBuff[5] >> 2) << 8) + _dataBuff[2] - _gyroY0;

return true;
}

return false;
}


private boolean calibrateGyrosZeroes() throws ConnectionLostException, InterruptedException {
long tempX=0, tempY=0, tempZ=0;
_gyroX0 = 0; _gyroY0 = 0; _gyroZ0 = 0;
for (int i=0; i<10; i++){
if (! readData()) return false;

tempZ += _gyroZ;
tempX += _gyroX;
tempY += _gyroY;

Thread.sleep(10);
}

// average 10 readings
_gyroZ0 = (int)(tempZ / 10);
_gyroX0 = (int)(tempX / 10);
_gyroY0 = (int)(tempY / 10);

return true;
}



public int getGyroX() {
return _gyroX;
}

public int getGyroY() {
return _gyroY;
}

public int getGyroZ() {
return _gyroZ;
}
}





MainActivity.java
-------------------------------------------------------
package com.trandi.wii;

import ioio.lib.api.PwmOutput;
import ioio.lib.api.TwiMaster;
import ioio.lib.api.exception.ConnectionLostException;
import ioio.lib.util.AbstractIOIOActivity;

import java.lang.Thread.UncaughtExceptionHandler;

import android.os.Bundle;
import android.util.Log;
import android.widget.FrameLayout;
import android.widget.TextView;


public class MainActivity extends AbstractIOIOActivity {
private TextView _varField;
private TextView _msg;
private FrameLayout _drawingFrame;
private Ball _ball;

/**
* Called when the activity is first created. Here we normally initialize our GUI.
*/
@Override
public void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
Thread.setDefaultUncaughtExceptionHandler(new UncaughtExceptionHandler() {
@Override
public void uncaughtException(Thread thread, Throwable ex) {
Log.e(Util.LOG_TAG, "", ex);
}
});

setContentView(R.layout.main);
_varField = (TextView) findViewById(R.id.textViewVarVal);
_msg = (TextView) findViewById(R.id.textViewMsg);

_drawingFrame = (FrameLayout) findViewById(R.id.drawingFrame);
_ball = new Ball(_drawingFrame.getContext(), 15);
_drawingFrame.addView(_ball);
}


/**
* This is the thread on which all the IOIO activity happens. It will be run
* every time the application is resumed and aborted when it is paused. The
* method setup() will be called right after a connection with the IOIO has
* been established (which might happen several times!). Then, loop() will
* be called repetitively until the IOIO gets disconnected.
*/
class IOIOThread extends AbstractIOIOActivity.IOIOThread {
private final IOIOWiiMotionPlus _wmp = new IOIOWiiMotionPlus();
private PwmOutput _led; // The on-board LED
private boolean _ledOn = false;



/**
* Called every time a connection with IOIO has been established.
* Typically used to open pins.
*
* @throws ConnectionLostException When IOIO connection is lost.
*/
@Override
protected void setup() throws ConnectionLostException {
try {
_led = ioio_.openPwmOutput(0, 300);
_led.setDutyCycle(0);

// initialize the I2C system, join the I2C bus,
TwiMaster twi = ioio_.openTwiMaster(0, TwiMaster.Rate.RATE_100KHz, false);
if(! _wmp.init(twi)) throw new Exception("Can't initialise WMP");
} catch (Exception e) {
msg(e);
}
}

/**
* Called repetitively while the IOIO is connected.
*
* @throws ConnectionLostException When IOIO connection is lost.
*/
@Override
protected void loop() throws ConnectionLostException {
try {
if(_wmp.readData()) {
_ball.setPos(mapPos(_wmp.getGyroX()), mapPos(_wmp.getGyroY()));

runOnUiThread(new Runnable() {
@Override
public void run() {
_varField.setText(_wmp.getGyroX() + "  " + _wmp.getGyroY() + "  " + _wmp.getGyroZ());
refreshBall();
}
});

_led.setDutyCycle(_ledOn ? 1 : 0);
_ledOn = ! _ledOn;

Thread.sleep(100);
}
} catch (Exception e) {
msg(e);
}
}
}

private static int mapPos(int value) {
return 150 + value / 30;
}

private void msg(Throwable e){
msg("ERR: " + e.getMessage());
}

private void refreshBall() {
runOnUiThread(new Runnable() {
@Override
public void run() {
_drawingFrame.removeView(_ball);
_drawingFrame.addView(_ball);
}
});
}

private void msg(final String msg){
runOnUiThread(new Runnable() {
@Override
public void run() {
_msg.setText("MSG:  " + msg);
}
});
}

/**
* A method to create our IOIO thread.
*
* @see ioio.lib.util.AbstractIOIOActivity#createIOIOThread()
*/
@Override
protected AbstractIOIOActivity.IOIOThread createIOIOThread() {
IOIOThread result = new IOIOThread();
return result;
}
}




Util.java
-------------------------------------------------------
package com.trandi.wii;

public class Util {
static final String LOG_TAG = "WII";

/**
* Transforms a 0..127 -128 .. 0 range into a 128..0..-128 one
*/
static int fixJavaByte(byte init){
int result = init;
boolean negative = result < 0;
if(negative) result = -result;
result = - result + 128;
if(negative) result = -result;
return result;
}
}




Ball.java
-------------------------------------------------------
package com.trandi.wii;

import android.content.Context;
import android.graphics.Canvas;
import android.graphics.Paint;
import android.view.View;

public class Ball extends View {
private float x = 0;
private float y = 0;
private final int r;
private final Paint mPaint = new Paint(Paint.ANTI_ALIAS_FLAG);

public Ball(Context context, int r) {
super(context);
mPaint.setColor(0xFFFF0000);
this.r = r;
}

public void setPos(float x, float y) {
this.x = x;
this.y = y;
}

@Override
protected void onDraw(Canvas canvas) {
super.onDraw(canvas);
canvas.drawCircle(x, y, r, mPaint);
}
}

 

A few things / tricky bits worth mentioning:

• when calling TwiMaster.writeRead() you HAVE to provide an output buffer IF the slave will output something (EVEN IF you don’t care about it and the output read size is 0)
• the Util.fixJavaByte() is important because in Java bytes are signed whereas in C they are not
• also notice the 2 addresses that the WMP uses: one for the init and the other one for the subsequent requests

The problem that I have is that the TWI library on the IOIO doesn’t time out if there’s no slave listening at the indicated address. This means that if I call init() and the WMP is already initialised, it will hang forever…