Go Down

Topic: original WMP+nunchuck clone passthrough (Read 4955 times) previous topic - next topic

lestofante

Hi,
i have a WMP and a nunchuck clone.
alone they work perfectly (nunchuck clone use a different init code from original one, but is ok).
Now, when i try to use together is pass-throug mode (they have the same i2c address, so this is the only way) i have some problem.

sometimes the WMP doesn't respond.
sometimes it work.
sometime it seems to work but nunchuk vales aren't update.
sometime it seems to work but nunchuk vales aren't update and are strange and high values (like 1020).

the weird things when "it seems to work" is that nunchuck response is received, but they have inconsistent data. i'm using a high delay, like 100ms, to be sure it isn't a communication problem.

connection are like the multiwii one, but using an external 3.3V regulator. No pull-up on i2c
Guida per principianti http://playground.arduino.cc/Italiano/newbie
Unoffical Telegram group https://t.me/genuino

lestofante

code:
Code: [Select]
// .......................................................................
// Code for reading data from a Wii Motion plus device connected to a Nunchuck
// Links to other sites
// http://www.windmeadow.com/node/42   .... first time i saw arduino nunchuk interface
// http://www.kako.com/neta/2009-017/2009-017.html# .....Japanese site with lots of Wii info
// http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus    .... the one and only
// http://randomhacksofboredom.blogspot.com/2009/06/wii-motion-plus-arduino-love.html
// ....original motion plus but not passthrough
// http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1248889032/35   .... great Kalman filter code
// thanks to duckhead and knuckles904. I will be using that code for sure.
// http://obex.parallax.com/objects/471/   .... ideas for passthrough
// by Krulkip
// Here is the bitmapping which is changed from the standard nunchuk mapping
// In Nunchuk mode:
//     Bit
// Byte     7 6 5 4 3 2 1 0
// 0 SX<7-----------------------------------------------------0>
// 1 SY<7-----------------------------------------------------0>
// 2 AX<9-----------------------------------------------------2>
// 3 AY<9-----------------------------------------------------2>
// 4 AZ<9---------------------------------------------3> 1
// 5 AZ<2-----1>     AY<1>   AX<1>   BC BZ 0 0
// Please note also the loss of the bit0 resolution.
// Byte 5 Bit 0 and 1 is used for nunchuk (0 0) = nunchuk or motion plus  (1 0) is motion plus detection.
// Byte 4 Bit 0 which is the extention controller detection bit. (1 = extension present)
// Hardware Arduino with ATMega 168
// Connections SCA to AD4 (Analog4) and SCL to AD5 (Analog5)
//........................................................................
#include <Wire.h>
#include <string.h>
#include <stdio.h>
uint8_t outbuf[6]; // array to store arduino output
int ledPin = 13;
int xID;
unsigned long lastRead=millis(),last=millis();
void setup (){
  Serial.begin (19200);
  Wire.begin();
  Serial.print ("Finished setup\n");
  Serial.print ("Now detecting WM+\n");
  delay(100);
  // now make Wii Motion plus the active extension
  // nunchuk mode = 05, wm+ only = 04, classic controller = 07
  Serial.print ("Making Wii Motion plus the active extension in nunchuk mode = 05 ........");
  Wire.beginTransmission(0x53);
  Wire.send(0xFE);
  Wire.send(0x05);// nunchuk
  Wire.endTransmission();
  Serial.print (" OK done");
  Serial.print ("\r\n");
  delay (100);
  // now innitiate Wii Motion plus
  Serial.print ("Innitialising Wii Motion plus ........");
  Wire.beginTransmission(0x53);
  Wire.send(0xF0);
  Wire.send(0x55);
  Wire.endTransmission();
  Serial.print (" OK done");
  Serial.print ("\r\n");
  delay (100);
  Serial.print ("Set reading address at 0xFA .......");
  Wire.beginTransmission(0x52);
  Wire.send(0xFA);
  Wire.endTransmission();
  Serial.print(" OK done");
  Serial.print ("\r\n");
  delay (100);
  Wire.requestFrom (0x52,6);
  outbuf[0] = Wire.receive();
  Serial.print(outbuf[0],HEX);
  Serial.print(" ");
  outbuf[1] = Wire.receive();
  Serial.print(outbuf[1],HEX);
  Serial.print(" ");
  outbuf[2] = Wire.receive();
  Serial.print(outbuf[2],HEX);
  Serial.print(" ");
  outbuf[3] = Wire.receive();
  Serial.print(outbuf[3],HEX);
  Serial.print(" ");
  outbuf[4] = Wire.receive();
  Serial.print(outbuf[4],HEX);
  Serial.print(" ");
  outbuf[5] = Wire.receive();
  Serial.print(outbuf[5],HEX);
  Serial.print(" ");
  Serial.print ("\r\n");
  xID= outbuf[0] + outbuf[1] + outbuf[2] + outbuf[3] + outbuf[4] + outbuf[5];
  Serial.print("Extension controller xID = 0x");
  Serial.print(xID,HEX);
  if (xID == 0xCB) {
    Serial.print (" Wii Motion plus connected but not activated");
  }
  if (xID == 0xCE) {
    Serial.print (" Wii Motion plus connected and activated");
  }
  if (xID == 0x00) {
    Serial.print (" Wii Motion plus not connected");
  }
  Serial.print ("\r\n");
  delay (100);
  // Now we want to point the read adress to 0xa40008 where the 6 byte data is stored
  Serial.print ("Set reading address at 0x08 .........");
  Wire.beginTransmission(0x52);
  Wire.send(0x08);
  Wire.endTransmission();
  Serial.print(" OK done");
  Serial.print ("\r\n");
 
  send_zero (); // send the request for next bytes
  lastRead=millis();
}

void send_zero (){
  Wire.beginTransmission(0x52);
  Wire.send(0x00);
  Wire.endTransmission();
}

int giro=0, nun=0, err=0;
void loop (){
  // now follows conversion command instructing extension controller to get
  // all sensor data and put them into 6 byte register within the extension controller
  if (lastRead+100 < millis()){
   
    //delay (100);
    Wire.requestFrom (0x52,6);
    outbuf[0] = Wire.receive();
    outbuf[1] = Wire.receive();
    outbuf[2] = Wire.receive();
    outbuf[3] = Wire.receive();
    outbuf[4] = Wire.receive();
    outbuf[5] = Wire.receive();

    if ((outbuf[5]&0x03)==0x00) {
      nun++;
      print ();
      //if (last+1000 <= millis() ){
      //  print ();
      //}
    }
    else if  ((outbuf[5]&0x03)==0x02){
      giro++;
    }
    else{
      err++;
    }
    send_zero (); // send the request for next bytes
    lastRead=millis();
  }
  if (last+1000 < millis() ){
    //    print ();
    Serial.print("giro:");
    Serial.print(giro);
    Serial.print(" nun:");
    Serial.print(nun);
    Serial.print(" err:");
    Serial.print(err);
    Serial.println();
    last=millis();
    giro=nun=err=0;
  }

}
void print () {
  // check if nunchuk is really connected
  if ((outbuf[4]&0x01)==0x01) {
    Serial.print("Ext con: ");
  }
  if ((outbuf[5]&0x03)==0x00) {
    int joy_x_axis = outbuf[0];
    int joy_y_axis = outbuf[1];
    int accel_x_axis = (outbuf[2] << 2) + ((outbuf[5] >> 3) & 2);
    int accel_y_axis = (outbuf[3] << 2) + ((outbuf[5] >> 4) & 2);
    int accel_z_axis = (outbuf[4] << 2) + ((outbuf[5] >> 5) & 6);
    int z_button = (outbuf[5]>>2) & 1;
    int c_button = (outbuf[5]>>3) & 1;
    //Serial.print ("joyx= ");
    //Serial.print (joy_x_axis);
    //Serial.print ("  joyy=");
    //Serial.print (joy_y_axis);
    Serial.print ("  accx= ");
    Serial.print (accel_x_axis);
    Serial.print ("  accy= ");
    Serial.print (accel_y_axis);
    Serial.print (" accz= ");
    Serial.print (accel_z_axis);
    Serial.print ("  ");
    if (z_button == 0) {
      Serial.print ("z_button ");
    }
    if (c_button == 0) {
      Serial.print ("c_button ");
    }
    Serial.print ("\r\n");
  }
  else
    if  ((outbuf[5]&0x03)==0x02) {
      int yaw = (((outbuf[5]&0xFC)<<6) + outbuf[0]);
      int pitch = (((outbuf[4]&0xFC)<<6) + outbuf[1]);
      int roll = (((outbuf[3]&0xFC)<<6) + outbuf[2]);
      Serial.print ("yaw= ");
      Serial.print (yaw);
      Serial.print ("  pitch= ");
      Serial.print (pitch);
      Serial.print ("  roll= ");
      Serial.print (roll);
      Serial.print ("\r\n");
    }

}

Guida per principianti http://playground.arduino.cc/Italiano/newbie
Unoffical Telegram group https://t.me/genuino

mcb1

lesto
I haven't played with mine for a while (I only just received the motor controller board).
I don't recall ever having read issues.

I have 2k7 pullups on the data and clock and use a 328.

Here is my code, it has a few corrections that were discovered on the way along the journey.
Code: [Select]
// .......................................................................
// Code for reading data from a Wii Motion plus device connected to a Nunchuck
// Links to other sites
// http://www.windmeadow.com/node/42   .... first time i saw arduino nunchuk interface
// http://www.kako.com/neta/2009-017/2009-017.html# .....Japanese site with lots of Wii info
// http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus    .... the one and only
// http://randomhacksofboredom.blogspot.com/2009/06/wii-motion-plus-arduino-love.html
// ....original motion plus but not passthrough
// http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1248889032/35   .... great Kalman filter code
// thanks to duckhead and knuckles904. I will be using that code for sure.
// http://obex.parallax.com/objects/471/   .... ideas for passthrough
// by Krulkip
// Here is the bitmapping which is changed from the standard nunchuk mapping
// In Nunchuk mode:
//     Bit
// Byte     7 6 5 4 3 2 1 0
// 0 SX<7-----------------------------------------------------0>
// 1 SY<7-----------------------------------------------------0>
// 2 AX<9-----------------------------------------------------2>
// 3 AY<9-----------------------------------------------------2>
// 4 AZ<9---------------------------------------------3> 1
// 5 AZ<2-----1>     AY<1>   AX<1>   BC BZ 0/1 0
// Please note also the loss of the bit0 resolution.
// Byte 5 Bit 1 is used to signify what data (0) = nunchuk, (1) is motion plus.
// Byte 4 Bit 0 is the extention controller detection bit. (1 = extension present)
// Hardware Arduino with ATMega 328
// Connections SCA to AD4 (Analog4) and SCL to AD5 (Analog5) with pullup of 2k7 to 3.3 volts
//........................................................................
// The Arduino I2C uses 7 bits, therefore the MP address (0xA6) becomes 0x53, and the (0xA4) becomes 0x52
// Modified by Mark Beckett
// May 2010

#include <Wire.h>
#include <string.h>
#include <stdio.h>
#include <LiquidCrystal.h>

//setup the variables, etc
int ledPin = 13;
int xID;

//Nunchuck
uint8_t outbuf[6];           //nunchuck buffer for data
int cnt = 0;                 //nunchuck buffer counter
int joy_x_axis;              //Joystick x (8 bits)
int joy_y_axis;              //Joystick y (8 bits)
int accel_x_axis;            //Nunchuck x (10 bits - bit0)
int accel_y_axis;            //Nunchuck y (10 bits - bit0)
int accel_z_axis;            //Nunchuck z (10 bits - bit0)

//Wii Motion Plus
byte data[6];                //buffer for WM+ data
int yaw, pitch, roll;        //three gyroscope axes (14 bits)
int yaw0, pitch0, roll0;     //calibration zeroes
bool yaw_v, pitch_v, roll_v; //calibration zeroes


// initialize the library with the numbers of the interface pins
LiquidCrystal lcd(7, 8, 9, 10, 11, 12);


void setup (){
  Serial.begin (19200);
// set up the LCD's number of rows and columns:
   lcd.begin(16, 2);
  //lcd.clear();
  lcd.setCursor(0,0);
  lcd.print ("Initialising");
  Wire.begin();
  Serial.print ("Finished setup\n");
  Serial.print ("Now detecting WM+\n");
  delay(100);
 
// innitiate Wii Motion plus
    Serial.print ("Initialising Wii Motion plus ........");
    Wire.beginTransmission(0x52); //0xA6
    Wire.send(0xF0);
    Wire.send(0x55);
    Wire.endTransmission();
    Serial.print (" OK done");
    Serial.print ("\r\n");
    delay (100);
 
// nunchuk mode = 05, wm+ only = 04, classic controller = 07
    Serial.print ("Initialising Wii Motion Plus for Nunchuk in passthru mode = 05 ........");
    Wire.beginTransmission(0x53); //0xA6
    Wire.send(0xFE);
    Wire.send(0x05);// nunchuk in pass thru mode
    Wire.endTransmission();
    Serial.print (" OK done");
    Serial.print ("\r\n");
    delay (100);
   
    Serial.print ("Set reading address at 0xFA .......");
    Wire.beginTransmission(0x52); //0xA4
    Wire.send(0xFA);
    Wire.endTransmission();
    Serial.print(" OK done");
    Serial.print ("\r\n");
    delay (100);
    Wire.requestFrom (0x52,6);  //0xA4
   outbuf[0] = Wire.receive();Serial.print(outbuf[0],HEX);Serial.print(" ");
   outbuf[1] = Wire.receive();Serial.print(outbuf[1],HEX);Serial.print(" ");
   outbuf[2] = Wire.receive();Serial.print(outbuf[2],HEX);Serial.print(" ");
   outbuf[3] = Wire.receive();Serial.print(outbuf[3],HEX);Serial.print(" ");
   outbuf[4] = Wire.receive();Serial.print(outbuf[4],HEX);Serial.print(" ");
   outbuf[5] = Wire.receive();Serial.print(outbuf[5],HEX);Serial.print(" ");
   Serial.print ("\r\n");
   xID= outbuf[0] + outbuf[1] + outbuf[2] + outbuf[3] + outbuf[4] + outbuf[5];
   //Serial.print("Extension controller xID = 0x");
   //Serial.print(xID,HEX);
   //delay (200);
   if (xID == 0xCB) { Serial.print (" Wii Motion plus connected but not activated"); } // 00 00 A6 20 00 05
   if (xID == 0xCE) { Serial.print (" Wii Motion plus connected and NC activated"); } // 00 00 A4 20 00 05
   if (xID == 0x00) { Serial.print (" Wii Motion plus not connected"); } // 00 00 00 00 00 00
   Serial.print ("\r\n");
   lcd.setCursor (0,0);
   lcd.print("Motion Plus OK");
   lcd.setCursor (0,1);
   lcd.print (" Nunchuck OK");
   delay (500);
   // Now we want to point the read adress to 0xa40008 where the 6 byte data is stored
    Serial.print ("Set reading address at 0x08 .........");
    Wire.beginTransmission(0x52);
    Wire.send(0x08);
    Wire.endTransmission();
    Serial.print(" OK done");
    Serial.print ("\r\n");
    lcd.clear();
  }
void send_zero (){
    Wire.beginTransmission(0x52);
    Wire.send(0x00);
    Wire.endTransmission();
}
void loop (){
// now follows conversion command instructing extension controller to get
// all sensor data and put them into 6 byte register within the extension controller

  send_zero (); // send the request for next bytes
  delay (100);
  Wire.requestFrom (0x52,6);
  outbuf[0] = Wire.receive();
  outbuf[1] = Wire.receive();
  outbuf[2] = Wire.receive();
  outbuf[3] = Wire.receive();
  outbuf[4] = Wire.receive();
  outbuf[5] = Wire.receive();
  print ();
  cnt = 0;

delay (100); //slow down the printing
}
void print (){
// check if nunchuk is really connected
// Byte(4) bit 0 is 1 if the extension is connected
  if ((outbuf[4]&0x01)==0x01) {
  Serial.print("Ext con: ");
  }

if ((outbuf[5]&0x03)==0x00) { //Nunchuck Data
int joy_x_axis = outbuf[0];
int joy_y_axis = outbuf[1];
int accel_x_axis = (outbuf[2] << 2) + ((outbuf[5] >> 3) & 2);
int accel_y_axis = (outbuf[3] << 2) + ((outbuf[5] >> 4) & 2);
int accel_z_axis = (outbuf[4] << 2) + ((outbuf[5] >> 5) & 6);
int z_button = (outbuf[5]>>2) & 1;
int c_button = (outbuf[5]>>3) & 1;
  Serial.print ("joyx= ");
  Serial.print (joy_x_axis, DEC);
  Serial.print ("  joyy=");
  Serial.print (joy_y_axis, DEC);
  Serial.print ("  accx= ");
  Serial.print (accel_x_axis, DEC);
  Serial.print ("  accy= ");
  Serial.print (accel_y_axis, DEC);
  Serial.print (" accz= ");
  Serial.print (accel_z_axis, DEC);
  Serial.print ("  ");
  if (z_button == 0) { Serial.print ("z_button "); }
  if (c_button == 0) { Serial.print ("c_button "); }
  Serial.print ("\r\n");
 
   //Send the Accelerometer data to the first line
  lcd.setCursor (0,0);
  lcd.print ("x"); lcd.print (accel_x_axis,DEC);
  lcd.setCursor (5,0);
  lcd.print ("y"); lcd.print (accel_y_axis,DEC);
  lcd.setCursor (10,0);
  lcd.print ("z"); lcd.print (accel_z_axis,DEC);
}
else if  ((outbuf[5]&0x03)==0x02) { //MP data
  int yaw = (((outbuf[5]&0xFC)<<6) + outbuf[2]);
  int pitch = (((outbuf[4]&0xFC)<<6) + outbuf[1]);
  int roll = (((outbuf[3]&0xFC)<<6) + outbuf[0]);
  Serial.print ("yaw= ");
  Serial.print (yaw, DEC);
  Serial.print ("  pitch= ");
  Serial.print (pitch, DEC);
  Serial.print ("  roll= ");
  Serial.print (roll, DEC);
  Serial.print ("\r\n");
 
  //Send the Gyro data to the second line
  lcd.setCursor (0,1);
  lcd.print ("w"); lcd.print (yaw,DEC);
  lcd.setCursor (5,1);
  lcd.print ("r"); lcd.print (roll,DEC);
  lcd.setCursor (10,1);
  lcd.print ("p"); lcd.print (pitch,DEC);
  Serial.print ("yaw= ");
  Serial.print (yaw, DEC);
  Serial.print ("  pitch= ");
  Serial.print (pitch, DEC);
  Serial.print ("  roll= ");
  Serial.print (roll, DEC);
  Serial.print ("\r\n");
}
}


Mark

massit78

I'm a beginner with Arduino, although I have experience in using C++, I don't understand the following part of the code

int accel_x_axis = (outbuf[2] << 2) + ((outbuf[5] >> 3) & 2);

what does the operators  "<<" ">>" mean in addition with "&2"?

can you post a simple example to understand the code?

lestofante

#4
Jul 31, 2011, 06:26 pm Last Edit: Jul 31, 2011, 06:27 pm by lesto Reason: 1
it's bitshift operation.
a byte for example is
01101001

if this byte is ouput[5] then
01101001 >> 3 = 00001101 (shiftet 3 bit on the left)
then biwise and:
00001101 & 2 = 00001101 & 00000010 (this is 2 in binary) = 0

maybe i'm wrong, but this 2 operation ban be just one:

01101001 & 00010000 (staring byte and 2^4=16)
so
ouput[5] & 16 == ((outbuf[5] >> 3) & 2)

as you can see, all this complexity is just to know if the 5° bit from the right is 1 or 0 :D
Guida per principianti http://playground.arduino.cc/Italiano/newbie
Unoffical Telegram group https://t.me/genuino

mcb1

lesto is quite right.

It's shifting bits in order to decide if the last two bits are a 1 or not.

Yes it can be a single operation, but then it gets hard to read, etc, etc.
The reality is writing tight code for a simple program isn't really necessary, and if you are still a novice, then it just makes your head hurt more.

@massit78
I found this very useful...now where did I put it.
http://www.arduino.cc/playground/Code/BitMath

Mark

lestofante

the code doesn't work.
It say nun is connected, but no data from it. also 0V on nunchuck's SDA... alone the nunchuck work correctly
Guida per principianti http://playground.arduino.cc/Italiano/newbie
Unoffical Telegram group https://t.me/genuino

mcb1

lesto
I presume it discovers the MP okay.
I noted in the Wiki that someone had to do some different intialisation for a 3rd party brand.

Have you tried the pullup resistors on SDA and SCL lines.?


Mark

lestofante

iv'e tried a pullup in sda line form WMP to nun, but then the WMP won't initialize, no response at all
Guida per principianti http://playground.arduino.cc/Italiano/newbie
Unoffical Telegram group https://t.me/genuino

mcb1

lesto
As far as i understand the I2C bus sytem is based on an open collector arrangement.
This enables any device to communicate.

The pullups should be fitted between SDA (A4) and 3v3, and SCL (A5) and 3v3 (on a 328 Arduino).
Note this reference uses 5v, but a Wii uses 3v3.....
http://arduino.cc/en/Tutorial/DigitalPotentiometer

Communication between the MP and NC should be sorted within the MP, so you shouldn't need any others.

Mark

Go Up