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…

About these ads

12 Responses to Android IOIO Wii Motion Plus – Gyroscopes

  1. giggs says:

    can I download a zip file of the code somewhere? pretty please?

    • trandi says:

      The code snippets that are in the post should be enough, this is just a demonstration, not a ready to use project.

  2. maty says:

    melding is mandatory?

    • trandi says:

      I’m not sure I understand your question… are you talking about what the Wii remote does by mixing the data from the WMP (gyroscopes) with the data from its own accelerometers ?
      Then NO, you can simply and purely get the gyro data directly…

      Dan

      • maty says:

        i mean did you meld the wires (this black box which hold the wires) on the IOIO board,that’s all

        also is the WII contain Accelerometer?
        thanks alot :D

      • trandi says:

        That “black box” is NOT a box, it’s simply a 3 rows connector, into which the wires plug. The first row is connected to the IOIO pins, the 2nd has VCC on it and the 3rd has GND. This way it’s easy to connect things like servos…

        The Wii MOTION PLUS (the device that I use here) contains ONLY gyroscopes. This device normally plugs into the “normal” wii remote which does have accelerometers. That was the whole point of the WMP, to add gyros to the controller for better / more accurate movement measurement.

        Hope this helps, Dan

  3. m.raikonnan says:

    is there is any chance it will burn the IOIO board,and did you find what you had done difficult?

    • trandi says:

      There is ALWAYS a “chance” to burn it, but in this case it sees quite low to me…
      As to the difficulty, it’s obviously highly relative to the experience / knowledge of the person :)
      I would personally say it’s “beginner / intermediate”.

      Hope this helps!
      Dan

      • m.raikonnan says:

        i need to talk to you in some issues,can you give me your mail or skype account if you have,thanks :D

  4. Pingback: Android IOIO Wii Motion Plus – Gyroscopes | PDA8

  5. Ytai says:

    Love your blog! Nice write-up.

    • trandi says:

      Thanks Ytai.
      And thank you for your quick reply:


      1. I’m a little surprised the writeRead() method doesn’t return with
      ‘false’ when there’s no response on the I2C bus. Does it hang even if
      there’s nothing physically connected?

      2. Either way, if you want to timeout, just interrupt() the waiting
      thread (e.g. from a TimerTask, which you can cancel if the operation did
      return). It’s been designed for that, and the blocking method will throw a
      InterruptedException.

      1. I haven’t tried with nothing PHYSICALLY connected, will do

      2. good to know, I’ll update my code and report back…

      dan

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

Follow

Get every new post delivered to your Inbox.

Join 132 other followers

%d bloggers like this: