Tuesday, November 16, 2010

Bus Notifier Followup

So since the Bus Notifier project was completed, it has been running from about Sept 2009 til now. Ive moved apartments, but the code still works with a few tweaks. The real news is:

1. I came in second in the Make Magazine and Design News sponsored Gadget Freak Design Contest. This was quite an honor (especially since I never win anything from contests). Heres the contest results page. Plus $500 bucks will go a long way to help out with some of my other projects.

2. Also...I got published in the Gadget Freak Section of the Design News Newsletter! Hooray. I had to do a professional photo shoot and everything. Also a huge honor (and another $500 for them using my submission). The site is here.

I am floored by all of this. To think that I got so much recognition from a simple project I designed to make my life easier is really amazing. Thanks so much to Make Magazine, Design News, and Hackaday for making all this possible.


P.S. In other news, I have a few projects I ought to post that have been finished for a while. I made a little scrolling LED matrix for my girlfriend last valentines day. It was something that I could use to learn to use the PIC's I sampled for free and she loves it since I can upload new messages to it whenever I want. Im a sap, what can I say.
I would also like to showcase my Isobot code properly eventually. It was an earlier project that Im quite proud of, but it never made it off the forum post aside from a quick make magazine writeup.
My biggest projects right now are school related: Building a quadcopter for a robotics class, my Mechanical Senior Design class, and looking for a job take up most of my time. The quadcopter is showcase on the tab above named "Horus Quadcopter Robot"

Saturday, April 10, 2010

Videos For My Bus Status Updater

Here are a few videos for the previous post. Sorry for the poor camera quality. All I have is a cameraphone to work with.



Most Useful Mess Of Wires: GPS Bus Notifier


Alright so I havent posted in a while, and the novelty of the wm+ has worn off. I dont have the funding to start a drone project right now, mainly since I’m a broke college student. And even if I did have the money, I dont think I would have the time this semester. I can barely manage to make it to classes right now. But it was from that problem that my last project took shape and has been helping me make it to my classes, making sure I dont fail out.

A little background info first: my city (Gainesville FL) recently put gps systems in most of their busses, which can be checked from the internet. Cool huh? It partially makes up for the fact that busses follow their schedule with an error of + or - 10 minutes (for a 40 minute route). While this is great, I found myself checking the computer every 30 seconds when I woke up, or my old computer would fail to shut down properly leaving it on all day while I wasnt home, not so good for a laptop. I wanted a way to be notified of when I actually had to get my butt out of the door and head to the bus, preferably without using my computer. Well, having just started to play around with the wiznet ethernet module (which comes on the arduino ethernet shield), I decided to make this my next project. So without further ado, I present the Bus Notifier!

I started by finding the static IP address for my bus's website, but since it is php based, the arduino obviously couldnt do anything with it. After mucking around, I found a text based version which was much more useful. The hardest parts of the project were creating a good string parsing algorithm to find just the info I wanted, and then figuring out which string tokens I wanted to make my program look for, signifying a change of state. The three states I decided upon were: nowhere near your apt, stopped somewhat close (for some reason my bus stops at one specific spot for like 10-15 minutes and just sits…yea really annoying) , and the last state is “the bus is coming, get out there!”. To allow me to know what state the bus(or busses) were in, I just used three leds, red, yellow, and green respectively. Later I added a piezo buzzer for the “get out there” state. It worked great for about three months in this state but eventually website updated and all my string parsing had to be redone. So on the next revision, I made the string parsing VERY easy to work with in case of later changes. It has been working this way every morning for the last three or so months.

Another kinda cool module I added was the ability to tweet the location of each of the busses every five minutes. I know this might seem stupid at first, but given the fact that twitter is very easy (and free) to check via text message, this allows me to check the location of the busses once I get out of class with nothing more than a “GET knuckles904” text message to twitter’s shortcode. It gives me the functionality of having a smartphone and internet plan without all the cost.

It took a lot of work, and several revisions to get everything working right but for the past 6 months, but of all the projects Ive spent time on, this has by far been the most useful. I feel I have actually gotten a return on my time investment on this one.

I am posting my most current code (just barely fits in a 168) below, as well as some links that are useful to see. Feel free to adapt any part of this code to your project. I am very proud of it, especially the new string searching function. Just please give credit where its due, and if possible link back here. Also as with all my other posts, any click to the “sponsors” on the right of the page is much appreciated.

http://www.arduino.cc/playground/Code/TwitterLibrary Makes Tweeting a cinch

http://ufl.transloc.com/ Gainesville’s GPS Bus website

http://ufl.transloc.com/t/ Text based version

http://www.nkcelectronics.com/wiznet-wiz812mj-tcpip-network-mod812.html Where I bought the Wiznet module

http://www.arduino.cc/en/Tutorial/TextString Libraries are there to make your lives easier

http://arduiniana.org/libraries/flash/ Thanks Mikal Hart for all your contributions

Dropboxed .pde of code I realized that the code wont necessarily work just by copy+paste, since there is embedded html in the pde. Use the scrollbox to view, and download the pde to run or modify.

CODE:







#include <Ethernet.h>
#include <WString.h>
#include <Twitter.h>
#include <Flash.h>

byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
byte ip[] = { 192, 168, 1, 111 };
byte busServer[] = {66, 199, 240, 114 };

#define green 3 //pin definitions for leds and buzzer
#define yellow 4
#define red 5
#define buzzer 6

#define clientRefresh 20000 //just refresh times
#define buzzerRefresh 15000
#define twitterRefresh 300000
/*
char yellowstr[]="SE 20th Pl"; //means bus is waiting
char greenstr1[]="4th St"; //by itself means bus is on the move towards apt
char greenstr2[]="State Hwy 331"; //these two must be together
char greenstr3[]="Southwest"; //" " mean that bus is on the way to apt
char tagstr[]="";
char redstr1[]="SE 16th Ave"; //means that bus is already gone if contains this
char redstr2[]="West"; //and west
char redstr3[]="North"; //or redstr1 and this
*/


String timestring(28); //holding strings
String bus1Locationstring(100); //
String bus2Locationstring(100); //

String timesearch(7);//searching strings
String busSearch(16);

uint8_t connectStatus;
uint8_t closeStatus;
char readvalue=0;
unsigned long timer1=0; //client request timer
unsigned long timer2=0; //client timeout timer
unsigned long timer3=0; //buzzer timer
unsigned long timer4=0; //twitter timer
int state=0;

//FLASH_STRING(msg1, "i like pie");

Client busClient(busServer, 80);
Twitter twitter("****:****");//see twitter library for description
void makestring(char c, String &s);
//String getbackstring(const prog_char* str);

void setup()
{
initEthernet();
timer1=millis();
timer3=millis();
timer4=millis();
ConnectToWebserver();
state=makestate(state);
//Serial.print(F("Status: "));
//Serial.println(state);
clearstrings();
initPins();
}

void loop(){
if ((millis()-timer1)>clientRefresh){
ConnectToWebserver();
state=makestate(state);
//Serial.print(F("Status: "));
//Serial.println(state);
twitterupdate();
clearstrings();
timer1=millis();
}
switch (state){
case 0:
blink(red, 1000);
break;
case 1:
turnon(green);
if((millis()-timer3)>buzzerRefresh){
buzz(1000, 100);
timer3=millis();
}
break;
case 2:
turnon(yellow);
break;
case 3:
turnon(red);
break;
}
//Serial.println((millis()-timer4));
delay(1);
}


void ConnectToWebserver(){

uint8_t connectStatus;
if (busClient.connect()) {

Serial.println(F("-Connected"));

// Send the HTTP GET to the server
busClient.println(F("GET /t/route.php?id=66 HTTP/1.0")); //id=?? for php address
busClient.println();

timer2 = millis(); //secondary timer to wait up to five seconds for the server to respond
while ((!busClient.available()) && ((millis() - timer2 ) < style="color: #777755;">//loop until we either get a response or five seconds is up
// Read the response
while(busClient.available()>0){
readvalue=busClient.read();
while(busClient.available()>0&&timesearch.equals("")==false){
makestring(readvalue, timesearch);
readvalue=busClient.read();
//do nothing until find timestart tag
}
while(busClient.available()>0&&readvalue!='<'){
makestring(readvalue, timestring);
readvalue=busClient.read();
}
timesearch="";
while(busClient.available()>0&&busSearch.equals("
Bus 1
"
)==false){
makestring(readvalue, busSearch);
readvalue=busClient.read();
//do nothing until find bus1start tag
}
while(busClient.available()>0&&readvalue!='<'){
makestring(readvalue, bus1Locationstring);
readvalue=busClient.read();
}
busSearch="";
while(busClient.available()>0&&busSearch.equals("
Bus 2
"
)==false){
makestring(readvalue, busSearch);
readvalue=busClient.read();
//do nothing until find bus2start tag
}
while(busClient.available()>0&&readvalue!='<'){
makestring(readvalue, bus2Locationstring);
readvalue=busClient.read();
}
busSearch="";
}
/*
if(!timestring.equals("")){
//Serial.println(timestring);

}
if(!bus1Locationstring.equals("")){
//Serial.println(bus1Locationstring);

}
if(!bus2Locationstring.equals("")){
//Serial.println(bus2Locationstring);

}*/

// Disconnect from the server
busClient.flush();
busClient.stop();
//Serial.println(F("Connection ended"));

}
else {
// Connection failed
//Serial.println(F(" - CONNECTION FAILED!"));
timer1=millis();
}
}

int makestate(int state){
if(((bus1Locationstring.contains("4th St")&&!bus1Locationstring.contains("1"))||(bus1Locationstring.contains("State Hwy 331")&&bus1Locationstring.contains("Southwest")))||
((bus2Locationstring.contains("4th St")&&!bus2Locationstring.contains("1"))||(bus2Locationstring.contains("State Hwy 331")&&bus2Locationstring.contains("Southwest")))){
state=1;
}
else if(bus1Locationstring.contains("SE 20th Pl")||bus2Locationstring.contains("SE 20th Pl")){
state=2;
}
else if( ( (bus1Locationstring.contains("SE 16th Ave")&&bus1Locationstring.contains("West")) || (bus1Locationstring.contains("SE 16th Ave")&&bus1Locationstring.contains("North")) )||
( (bus2Locationstring.contains("SE 16th Ave")&&bus2Locationstring.contains("West")) || (bus2Locationstring.contains("SE 16th Ave")&&bus2Locationstring.contains("North")) ) ){
state=3;
}
return state;
}

void makestring(char inChar, String &inString) {
int maxLength=inString.capacity();
// read the incoming data as a char:
// if you're not at the end of the string, append
// the incoming character:
if (inString.length() < style="color: rgb(204, 102, 0);">else { //rolling string, taking out the first char and putting in a new one at the end
for(int i=0;ivoid buzz(int period, int number){
for (int i=0;idigitalWrite(buzzer, LOW);
delayMicroseconds(period/2);
digitalWrite(buzzer, HIGH);
delayMicroseconds(period/2);
}
}//we use 1000,100 for now

void blink(int pin, int duration){
digitalWrite(pin, HIGH);
delay(duration/2);
digitalWrite(pin, LOW);
delay(duration/2);
}

void initEthernet(){
pinMode(8, OUTPUT);
digitalWrite(8, HIGH);
delay(100);
digitalWrite(8, LOW);
delay(100);
digitalWrite(8, HIGH);
delay(5000);
Ethernet.begin(mac, ip);
Serial.begin(115200);
Serial.print(F("Start"));
}

void initPins(){
pinMode(green, OUTPUT);
pinMode(yellow, OUTPUT);
pinMode(red, OUTPUT);
pinMode(buzzer, OUTPUT);
digitalWrite(green, LOW);
digitalWrite(yellow, LOW);
digitalWrite(red, LOW);
}

void clearstrings(){
timestring="";
bus1Locationstring="";
bus2Locationstring="";
}

void turnon(int pin){
digitalWrite(green, LOW);
digitalWrite(yellow, LOW);
digitalWrite(red, LOW);
digitalWrite(pin, HIGH);
}

void turnoff(){
digitalWrite(green, LOW);
digitalWrite(yellow, LOW);
digitalWrite(red, LOW);
}

void twitterpost(int busnumber){
boolean good=0;
//Serial.println(F("connecting ..."));
if(busnumber==1){
good=twitter.post(bus1Locationstring);
}
else if(busnumber==2){
good=twitter.post(bus2Locationstring);
}
if (good) {
int status = twitter.wait();
if (status == 200) {
//Serial.println(F("OK.1"));
}
else {
//Serial.print(F("failed : code "));
//Serial.println(status);
}
}
else {
//Serial.println(F("connection failed."));
}
twitter.stop();
delay(1000);
}


void twitterupdate(){
if((millis()-timer4)>twitterRefresh){
if(!bus1Locationstring.equals("")){
twitterpost(1);
buzz(1000, 100);
delay(1000);
buzz(1000, 100);
delay(1000);
buzz(1000, 100);
}
if(!bus2Locationstring.equals("")){
twitterpost(2);
buzz(1000, 100);
delay(1000);
buzz(1000, 100);
delay(1000);
buzz(1000, 100);
}
timer4=millis();
}
}
/*
String getbackstring(const prog_char* str){
String tempstring(30);
//str.copy(tempstring, str.length());
return tempstring;
}
*/
/* //this whole sketch is based on the new ethernet client example
int ReadResponse(){
int totalBytes=0;
unsigned long timer2 = millis();

// First wait up to 5 seconds for the server to return some data.
// If we don't have this initial timeout period we might try to
// read the data "too quickly" before the server can generate the
// response to the request

while ((!busClient.available()) && ((millis() - timer2 ) <>

while (busClient.available()) {
char c = busClient.read();
Serial.print(c);
totalBytes+=1;
}
return totalBytes;
}
*/


Monday, February 15, 2010

WM+ update long overdue

Well I havent posted here for a while, but I do have many updates for a couple different projects. I never ended up getting full kalman filtering to work between the wm+, nunchuck, and arduino, but there have been a few other, maybe better things. First, my transistor method is no longer needed to interface the wm+ and the nunchuck at the same time. Pass thru mode has been discovered and is described (krulkip) in this post: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1248889032/45 . Second, the kalman method has been mostly abandoned on the arduino platform, and in its place two options have surfaced.
1. Complimentary filtering as seen here .
2. Direction Cosine Matrix. Not implemented with wii peripherals but easily enough ported. Tons of information over at diydrones.com . Much less computationally intense and more suited to a microcontroller like the arduino.

Good luck on all your projects!

Sunday, July 26, 2009

Motion Plus and Nunchuck together on the Arduino


Easy as 1-2-3

Alright, one great aspect about the Wii Motion Plus is its pass through port for other extension controllers such as the nunchuck. Unfortunately, no one has been able to read both an active motion plus and any other controller at the same time because they are all on the same I2C address(smooth nintendo). This is creates a large hurdle to people like myself who bought the WM+ to create a low cost IMU with it and the accelerometer in the nunchuck. After a good deal of digging and very little luck, I did find a way to use both at the same time(though not through the pass through port). And best of all its cheap!

Note before continuing: Please remember that I am disclosing this method free of cost in the spirit of open sourciness. As such, if you read this and it helps you, please help me a little by clicking on one(or more) of the ads to the side. It will help me out(monetarily) and let me feel like Ive gotten a little back for publishing it instead of keeping it to myself. Dont worry, they wont bite (theyre google ads).

Credit where credit is due: I got the idea from
this post, so many thanks johnnyonthespot

Materials:
2 NPN type switching transistors (I used 2N2222-super easy to find)
2 current limiting resistors (I used 470 Ohm)
Breadboard and jumper wires or make your own board

All of these materials can be easily and very cheaply bought at radioshack, mouser, digikey, or the like.
Literally, it will cost like $4.00 if you already have a breadboard


Rigging it Up:
Here is a simple schematic for breadboarding:


Code:
#include <Wire.h>
#include <Streaming.h>

//WM+ stuff
byte data[6]; //six data bytes
int yaw, pitch, roll; //three axes
int yaw0, pitch0, roll0; //calibration zeroes

//nunchuck stuff
uint8_t outbuf[6]; // array to store arduino output
int cnt = 0;
int joy_x_axis, joy_y_axis, accel_x_axis, accel_y_axis, accel_z_axis;
boolean z_button, c_button;

void setup(){
Serial.begin(115200);
Serial << "WM+ and Nunchuck tester" << endl;
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
digitalWrite(3, HIGH);
digitalWrite(4, HIGH);
Wire.begin();
switchtowmp();
wmpOn();
calibrateZeroes();
switchtonunchuck();
nunchuck_init();
}

void loop(){
switchtowmp();
receiveData();
//receiveRaw();
switchtonunchuck();
receive_nunchuck_data();
Serial << yaw << "\t" << pitch << "\t" << roll << "\t" << joy_x_axis << "\t" << joy_y_axis << "\t";
Serial << accel_x_axis << "\t" << accel_y_axis << "\t" << accel_z_axis;
Serial << "\t" << _DEC(z_button) << "\t" << _DEC(c_button) << endl;
delay(100);
}

void nunchuck_init ()
{
Wire.beginTransmission (0x52);// transmit to device 0x52
Wire.send (0x40); // sends memory address
Wire.send (0x00); // sends sent a zero.
Wire.endTransmission (); // stop transmitting
}

void send_zero ()
{
Wire.beginTransmission (0x52);// transmit to device 0x52
Wire.send (0x00); // sends one byte
Wire.endTransmission (); // stop transmitting
}

void receive_nunchuck_data(){
Wire.requestFrom(0x52, 6);
for (int i=0;i<6;i++){
data[i]=Wire.receive();
}
make_nunchuck_data();
send_zero();
}

void make_nunchuck_data ()
{
for(int i=0;i<6;i++){
outbuf[i]=nunchuck_decode_byte(data[i]);
}
joy_x_axis = outbuf[0];
joy_y_axis = outbuf[1];
accel_x_axis = outbuf[2] * 2 * 2;
accel_y_axis = outbuf[3] * 2 * 2;
accel_z_axis = outbuf[4] * 2 * 2;
z_button = 0;
c_button = 0;

// byte outbuf[5] contains bits for z and c buttons
// it also contains the least significant bits for the accelerometer data
// so we have to check each bit of byte outbuf[5]
if ((outbuf[5] >> 0) & 1)
{
z_button = 1;
}
if ((outbuf[5] >> 1) & 1)
{
c_button = 1;
}

if ((outbuf[5] >> 2) & 1)
{
accel_x_axis += 2;
}
if ((outbuf[5] >> 3) & 1)
{
accel_x_axis += 1;
}

if ((outbuf[5] >> 4) & 1)
{
accel_y_axis += 2;
}
if ((outbuf[5] >> 5) & 1)
{
accel_y_axis += 1;
}

if ((outbuf[5] >> 6) & 1)
{
accel_z_axis += 2;
}
if ((outbuf[5] >> 7) & 1)
{
accel_z_axis += 1;
}
}

char nunchuck_decode_byte (char x)
{
x = (x ^ 0x17) + 0x17;
return x;
}

void wmpOn(){
Wire.beginTransmission(0x53);//WM+ starts out deactivated at address 0x53
Wire.send(0xfe); //send 0x04 to address 0xFE to activate WM+
Wire.send(0x04);
Wire.endTransmission(); //WM+ jumps to address 0x52 and is now active
}

void wmpOff(){
Wire.beginTransmission(82);
Wire.send(0xf0);//address then
Wire.send(0x55);//command
//Wire.send(0x00);
//Wire.send(0xfb);
Wire.endTransmission();
}

void wmpSendZero(){
Wire.beginTransmission(0x52);//now at address 0x52
Wire.send(0x00); //send zero to signal we want info
Wire.endTransmission();
}

void calibrateZeroes(){
for (int i=0;i<10;i++){
wmpSendZero();
Wire.requestFrom(0x52,6);
for (int i=0;i<6;i++){
data[i]=Wire.receive();
}
yaw0+=(((data[3] >> 2) << 8)+data[0])/10;//average 10 readings
pitch0+=(((data[4] >> 2) << 8)+data[1])/10;// for each zero
roll0+=(((data[5] >> 2) << 8)+data[2])/10;
}
Serial.print("Yaw0:");
Serial.print(yaw0);
Serial.print(" Pitch0:");
Serial.print(pitch0);
Serial.print(" Roll0:");
Serial.println(roll0);
}

void receiveData(){
wmpSendZero(); //send zero before each request (same as nunchuck)
Wire.requestFrom(0x52,6); //request the six bytes from the WM+
for (int i=0;i<6;i++){
data[i]=Wire.receive();
}
//see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
//for info on what each byte represents
yaw=((data[3] >> 2) << 8)+data[0]-yaw0;
pitch=((data[4] >> 2) << 8)+data[1]-pitch0;
roll=((data[5] >> 2) << 8)+data[2]-roll0;
}

void receiveRaw(){
wmpSendZero(); //send zero before each request (same as nunchuck)
Wire.requestFrom(0x52,6);//request the six bytes from the WM+
for (int i=0;i<6;i++){
data[i]=Wire.receive();
}
yaw=((data[3] >> 2) << 8)+data[0];
pitch=((data[4] >> 2) << 8)+data[1];
roll=((data[5] >> 2) << 8)+data[2];
}

void switchtonunchuck(){
digitalWrite(3, LOW);
digitalWrite(4, LOW);
digitalWrite(4, HIGH);
}

void switchtowmp(){
digitalWrite(3, LOW);
digitalWrite(4, LOW);
digitalWrite(3, HIGH);
}


Closing Comments
If some of this code looks wierd to you its probably the streaming operator(<<). I prefer to use this form from mikalhart's streaming library. If you dont like it or dont want to download it, just replace any << with a .print() of the type specified before the << operator (ex: Serial << "hi" << endl; is the same as Serial.print("hi"); Serial.println(); )

Also remember to reference the definitions of the axes for each sensor; yaw, pitch, and roll are not the same for both, nor are they necessarily what I would have picked them to be. Im just keeping with what prior work has named them. Also, if you want a full fledged IMU from these components, stay tuned. I should have a decent
kalman filtered IMU demo coming soon. Woot!

Lastly, I find the Arduino.cc forum to be much better at handling questions so try to reach me there on my post in the exhibition section if your comment here doesnt get answered.

Monday, June 22, 2009

Wii Motion Plus + Arduino = Love

Ok so I, after much research, have been able to read the gyro data of the new wii motion plus peripheral with the arduino microcontroller. With this code and the code previously developed for the wii nunchuck (here), we are able to create a 6 DOF IMU for under $40. Thanks Nintendo! Best of all, everything is I2C so only 2 analog inputs (A4 and A5 needed for the wire library) are needed to read 6 sensors and no ADC conversion happens on the arduino board.

Links and Thanks:
First off, I'm going to link(and thank) a few sites that have helped me immensely with this undertaking:
Arduino.cc
original nunchuck code
Wii brew WM+ info
Propeller forums

Hardware connections:
I used full 5V TTL signals and power and have had no problems thus far (a week going strong) but if you have a 3.3v arduino or a level converter and 3.3v regulator, i would suggest that over a 5v connection. Furthermore, twi.h under the wire library does not need to be changed for my setup but might under some setups. Start with the default 100khz TWI_FREQ and if that doesnt work, use 400khz. Websites disagree about which is the proper speed for direct I2C on wii peripherals. Connections to WM+ are same as nunchuck and look like this:
| 1 2 3 |
| |
| 6 5 4 |
|_-----_|

1 - green - data
2 - nothing
3 - red - 3.3+v
4 - yellow - clock
5 - nothing
6 - white - ground

So its pin 3 to 5v, 6 to ground, 1 to A4, and 4 to A5

Adapters such as the one sold here should (theoretically) work; I use jumper wires and hot glue

Software:

I have commented the demo code pretty well so it should be easy to follow, if not comment here or on Arduino.cc Forums->Exhibition

#include <Wire.h>
byte data[6]; //six data bytes
int yaw, pitch, roll; //three axes
int yaw0, pitch0, roll0; //calibration zeroes

void wmpOn(){
Wire.beginTransmission(0x53); //WM+ starts out deactivated at address 0x53
Wire.send(0xfe); //send 0x04 to address 0xFE to activate WM+
Wire.send(0x04);
Wire.endTransmission(); //WM+ jumps to address 0x52 and is now active
}

void wmpSendZero(){
Wire.beginTransmission(0x52); //now at address 0x52
Wire.send(0x00); //send zero to signal we want info
Wire.endTransmission();
}

void calibrateZeroes(){
for (int i=0;i<10;i++){
wmpSendZero();
Wire.requestFrom(0x52,6);
for (int i=0;i<6;i++){
data[i]=Wire.receive();
}
yaw0+=(((data[3]>>2)<<8)+data[0])/10; //average 10 readings
pitch0+=(((data[4]>>2)<<8)+data[1])/10;
roll0+=(((data[5]>>2)<<8)+data[2])/10;
}
Serial.print("Yaw0:");
Serial.print(yaw0);
Serial.print(" Pitch0:");
Serial.print(pitch0);
Serial.print(" Roll0:");
Serial.println(roll0);
}

void receiveData(){
wmpSendZero(); //send zero before each request (same as nunchuck)
Wire.requestFrom(0x52,6); //request the six bytes from the WM+
for (int i=0;i<6;i++){
data[i]=Wire.receive();
}
yaw=((data[3]>>2)<<8)+data[0]-yaw0;
pitch=((data[4]>>2)<<8)+data[1]-pitch0;
roll=((data[5]>>2)<<8)+data[2]-roll0;
}
//see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
//for info on what each byte represents
void setup(){
Serial.begin(115200);
Serial.println("WM+ tester");
Wire.begin();
wmpOn(); //turn WM+ on
calibrateZeroes(); //calibrate zeroes
delay(1000);
}

void loop(){
receiveData(); //receive data and calculate yaw pitch and roll
Serial.print("yaw:");//see diagram on randomhacksofboredom.blogspot.com
Serial.print(yaw); //for info on which axis is which
Serial.print(" pitch:");
Serial.print(pitch);
Serial.print(" roll:");
Serial.println(roll);
delay(100);
}



Orientation:

Sorry for the arrows being all in the negative direction. Picture was done quickly.

Image and video hosting by TinyPic

ENJOY!