Jump to content
Sign in to follow this  
FbForos

DIY HTWm+

Recommended Posts

Dear colleagues,

invited by VRFlyer in posting here too our DIY HT

We developed a Head traker based in Arduino and WII motion plus (WM+). The name for this machine is HTWMplus

My colleague Cacer from Spain had the idea and Nono from Argentina write the initial program.

The WM+ has 3 gyros at a real low price 20$ more or less. Now the system is working only for Radios in which is possible to select the channels that will be used by the trainer port.

We wrote the program for the rest of the radios and work perfectly without the RF module (35 / 40 / 72 Mhz) , but as soon as we connect it, the interferences block the system. We are working for solution this problem

In the photo attached you will find the schema and here is the code. The WM+ need only 3.3 V but a lot of us, use 5V of arduino as power supply. Anyway I'm using a LDO 2950 - 3.3

/*#####################################################################################

## Wii HeadTracker    Version:  beta 3.02                          Diciembre 2009    ##

##                                                                                   ##

## Autor: El Nono, Cacer, Fbforos y el foro                                          ##

## Colaboracion:  www.aeromodelismovirtual.com                                       ##

##                www.fpv-bsas.com.ar                                                ##

## Documentacion:                                                                    ##

##                                                                                   ##

## http://www.aeromodelismovirtual.com/showthread.php?t=4300                         ##

## http://randomhacksofboredom.blogspot.com/2009/06/wii-motion-plus-arduino-love.html##

## http://www.windmeadow.com/node/42                                                 ##

## http://ingenegros.com.ar/Microcontroladores/leer-un-wii-nunchuck-con-arduino.html ##

## http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus             ##

#####################################################################################*/


//see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus

//for info on what each byte represents


/* Yaw data[]    3 y 0 

   Pitch data[]  4 y 1

   Roll data[]   5 y 2

   Estos Pich y Roll pueden intercambiarse según se monte el sensor para hacerlo 

   Buscais las lineas donde aparece data[4]   data[1] y las sustituis por 

   data[5] y data[2] o viceversa

*/

#define FB    //Coge mis parámetros del servo, comentar si quereis que los servos

             // tengan recorrido estandar.


#include <Wire.h>


// ************** OPCIONES DE CONFIGURACION ****************************

int pitch_reverse = 0;  // Invierte servo TILT   0 = normal  1 = Reverse

int yaw_reverse = 1;    // Invierte servo PAN    0 = Normal  1 = Reverse

int pitch_gan = 200;     // Relación entre angulo girado por la cabeza y el girado por

int yaw_gan = 200;       // el servo a mayor x_gan menor es en angulo girado

int SwitchPin = 5;      // Ubicacion del interrupter de centrado

                        // conectar un pulsador NA a masa


int ppmOutPin = 4 ;    // Salida de datos al trainner port

int ppmInPin = 2 ;     // Entrada de datros del trainner port futura actualizacion otras frec. distintas de

                      // 35 y 72

int canales = 6;       // Canal a partir del cual añadiremos el HT  / Canales que se generan                       


// Variables actualización


int refreshTime1= 8;    // Intervalo de generar trama 

long lastPulse1 = 0;


// Recorrido maximos de los servos - -2000, 2000 corresponde

// a un pulso de 1000 a 2000ms  y es el estandar

// Modificando los valores podemos ajustar el recorrido del servo

// no utilizar valores por encima de +-2500 puede no funcionar


#if defined FB

  int pitch_min = -2200; // TILT Minimo

  int pitch_max = 900;  //      maximo

  int yaw_min = -2300;   // PAN  Minimo

  int yaw_max = 2400;    //      Maximo

#else

  int pitch_min = -2000; // TILT Minimo

  int pitch_max = 2000;  //      maximo

  int yaw_min = -2000;   // PAN  Minimo

  int yaw_max = 2000;    //      Maximo

#endif


//**********************************************************************


volatile int estado = 1 ;

volatile int numPulsos= 0;

volatile int reposo=0;

volatile unsigned long time=0;


byte data[6]; //six data bytes

int yaw, pitch;

int yaw0, pitch0; //calibration zeroes

long pitch_bak1, yaw_bak1; //acumula el movimiento

int pitch_bak2, pitch_bak3; // variables auxiliares

int yaw_bak2, yaw_bak3 ;

int ledPin = 13;


int pulseWidthP = 0, pulseWidthY = 0;

int minPulseP = 1500;

int minPulseY = 1500;

//int dtime = 10;

//int t = 0;

int servoPin=7, servoPin2=6; //Salida de pruebas para servos


void wmpOn()  // Inicializa el Wii Motionplus

{

  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(){  // Proceso de calibracion del motionplus al inicio


  for (int i=0;i<300;i++)       //Vacía el buffer del WM+ de basura

  {                             // antes de calibrar el dispositivo

    wmpSendZero();

    Wire.requestFrom(0x52,6);

    for (int i=0;i<6;i++){

      data[i]=Wire.receive();

    } 

  }


  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[5]>>2)<<8)+data[2])/10;


  }

}


void receiveData()  // Recibe y procesa los datos del motionplus

{

  digitalWrite(ledPin, HIGH);  

  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();

  }

  digitalWrite(ledPin, LOW);


  if (data[4]&&2){

      pitch=((data[5]>>2)<<8)+data[2]-pitch0; 

      if (abs(pitch) <30) pitch=0;     

  } 

  else

  {

      pitch=(((data[5]>>2)<<8)+data[2]-pitch0)*5;

  }  

  if (data[3]&&2){

      yaw=((data[3]>>2)<<8)+data[0]-yaw0;

      if (abs(yaw )<30) yaw=0;

  }

  else

  {

      yaw=(((data[3]>>2)<<8)+data[0]-yaw0)*5;

  }

  if (pitch_reverse == 1) pitch*=-1;

  if (yaw_reverse == 1) yaw*=-1;

}


void updateServo()  // Calcula el ancho del pulsos para cada servo

{

      // ************** pitch *******************************

     pitch_bak2= (pitch_bak1 += pitch)/pitch_gan;

     pitch_bak3 = constrain((pitch_bak1/ pitch_gan),pitch_min,pitch_max);

     pulseWidthP = (pitch_bak3/4)+minPulseP;


    // ************* yaw **********************************    

     yaw_bak2=(yaw_bak1+=yaw)/yaw_gan  ;

     yaw_bak3 = constrain(yaw_bak2,yaw_min,yaw_max);  

     pulseWidthY = (yaw_bak3/4)+minPulseY;


    // ************ Control para pruebas *********************

    // ** Quitar el comentario a las 2 lineas siguiente para monitorizar con el arduino *******

    // ** Recordar que hay que comentar de nuevo antes del uso definitivo


    //Serial.print("\t pulse: ");   Serial.print(yaw); Serial.print("\t ");Serial.print(yaw_bak1); 

    //Serial.print("\t ");  Serial.print(yaw_bak2); Serial.print("\t ");Serial.print(yaw_bak3); Serial.print("\t "); Serial.println(pulseWidthY);  

}


void updatePPM()  // Añade los canales del HT 

{

     // >>>> Genera los primeros canales ficticios <<<<<<<<<

    for (int i=0; i < canales; i++)   // Envia el pulso ficticio de nº canales

    {

      digitalWrite(ppmOutPin,LOW);

      delayMicroseconds(400);

      digitalWrite(ppmOutPin, HIGH);

      delayMicroseconds(650);

    }

    digitalWrite(ppmOutPin,LOW);

    delayMicroseconds(400);


    // >>>> Canal "pan" (Canales+1)  <<<<<<<<<

    digitalWrite(ppmOutPin,HIGH);

    digitalWrite(servoPin2, HIGH);    

    delayMicroseconds(pulseWidthY-400);


    // Pulso de separacion de canales 

    digitalWrite(ppmOutPin,LOW);

    delayMicroseconds(400);

   digitalWrite(servoPin2, LOW);

   digitalWrite(servoPin, HIGH);


    // >>>> Canal "tilt" (Canales+2) <<<<<<<<<

    digitalWrite(ppmOutPin,HIGH);

    delayMicroseconds(pulseWidthP-400);


   // Pulso de separacion de canales 

    digitalWrite(ppmOutPin,LOW);

    delayMicroseconds(400);

    digitalWrite(ppmOutPin,HIGH);

    digitalWrite(servoPin, LOW);

}


void setup()

{

  Serial.begin(115200);

  pinMode(ppmOutPin, OUTPUT);  

  pinMode(ppmInPin, INPUT);   

  pinMode(ledPin, OUTPUT);  

  pinMode(servoPin, OUTPUT);   

  pinMode(servoPin2, OUTPUT);  

  pinMode(SwitchPin, INPUT);

  digitalWrite(SwitchPin,HIGH); //Pull_up entrada switch centrado

  digitalWrite(ppmOutPin,HIGH);

  Wire.begin();

  wmpOn(); //turn WM+ on

  digitalWrite(ledPin, HIGH);

  calibrateZeroes(); //calibrate zeroes

  digitalWrite(ledPin, LOW);

}


void loop()

{

    receiveData(); 

    updateServo();

    if (millis() - lastPulse1 >= refreshTime1){   // Genera la trama PPM

        updatePPM(); 

        lastPulse1 = millis();

       } 

   if (digitalRead(SwitchPin) == 0) {

    pulseWidthP = 1500;

    pitch_bak1 = 0;

    pitch_bak2=0;

    pulseWidthY = 1500;

    yaw_bak1 = 0;

    yaw_bak2 =0;

  }

}

Only one problem, the program is commented in spanish. If somebody need help we can translate to English

post-5814-126140635486_thumb.png

Share this post


Link to post
Share on other sites

That is a nice use for the WM+. Does your WM+ project work as well as the popular gyro based head trackers? The reason I ask is because I was looking at the wiibrew information and it mentioned this:

"The combination of 3 linear accelerations with 3 angular rates allows what Nintendo refers to as 1:1 motion tracking, which is another way of saying 6DOF (degrees of freedom) over a short time. It's only valid over short times because of the integration involved to convert accelerations and rates into positions (input errors, when integrated, blow up over time)."

Thanks for posting the code and schematic.

Share this post


Link to post
Share on other sites

The WM+ is nothing but a 3-axis gyro reporting a 16bit raw digital value for each axis. The 6DOF trickery needs the accelerometer that is in the Wiimote itself, and for absolute referencing there's the IR camera viewing the sensor bar's 2 dots.

Share this post


Link to post
Share on other sites
The WM+ is nothing but a 3-axis gyro ... snip ... The 6DOF trickery needs the accelerometer that is in the Wiimote itself

Thanks for the details.

Share this post


Link to post
Share on other sites

The WM+ is nothing but a 3-axis gyro reporting a 16bit raw digital value for each axis. The 6DOF trickery needs the accelerometer that is in the Wiimote itself, and for absolute referencing there's the IR camera viewing the sensor bar's 2 dots.

Kilrah,

I agreed with you that for 6DOFn accelerometers are necessary, but for this application only with the gyros we have enough precision. The system is working in version Beta 3.2 without any problem.

Share this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now
Sign in to follow this  

×