Arduino+Wii Motion Plus

[EvgenDRV]

Доброго времени суток! Подскажите пожалуйста как подключить несколько (два или три) WMP к одной Arduino? Один подключить можно к 4 и 5 ноге анал-о входа, но как нужно подправить скетч чтобы можно было подключить ещё WMP допустим к 3 и 2 ноге?

#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 <a href="http://wiibrew.org/wiki/Wiimote/Extension_Controller..." title="http://wiibrew.org/wiki/Wiimote/Extension_Controller..." rel="nofollow">http://wiibrew.org/wiki/Wiimote/Extension_Controller...</a>
//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("\r\n");
Serial.print(pitch);
Serial.print("\r\n");
Serial.println(roll);
delay(100);
}

[Реклама]

Реклама: ООО ТД Промэлектроника, ИНН: 6659197470, Тел: 8 (800) 1000-321

[admin]

Приведи полностью скетч или страничку откда его взял. Это не весь код.

[Реклама]

20% скидка на весь каталог электронных компонентов в ТМ Электроникс!

Акция "Лето ближе - цены ниже", успей сделать выгодные покупки!

Плюс весь апрель действует скидка 10% по промокоду APREL24 + 15% кэшбэк и бесплатная доставка!

Перейти на страницу акции

Реклама: ООО ТМ ЭЛЕКТРОНИКС, ИНН: 7806548420, info@tmelectronics.ru, +7(812)4094849

[EvgenDRV]

Приведи полностью скетч или страничку откда его взял. Это не весь код.

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

У меня скетч так и работает... копировал-вставил всё, а что к чему непонятно и откуда берутся данные с пятой ноги... ???

Изменено 13 сентября, 2011 пользователем EvgenDRV

[Реклама]

Выбираем схему BMS для корректной работы литий-железофосфатных (LiFePO4) аккумуляторов

 Обязательным условием долгой и стабильной работы Li-FePO4-аккумуляторов, в том числе и производства EVE Energy, является применение специализированных BMS-микросхем. Литий-железофосфатные АКБ отличаются такими характеристиками, как высокая многократность циклов заряда-разряда, безопасность, возможность быстрой зарядки, устойчивость к буферному режиму работы и приемлемая стоимость. Но для этих АКБ, также как и для других, очень важен контроль процесса заряда и разряда, а специализированных микросхем для этого вида аккумуляторов не так много. Инженеры КОМПЭЛ подготовили список имеющихся микросхем и возможных решений от разных производителей. Подробнее>>

Реклама: АО КОМПЭЛ, ИНН: 7713005406, ОГРН: 1027700032161

[admin]

На 4 и 5 ноге находится Wire. Вот оттуда данные и берутся. Поменять что-либо нельзя.

[EvgenDRV]

Понял... жаль... спасибо!

Вот нарыл скетч для WMP и Нунчаки, всё бы хорошо и этого в принципе достаточно, но когда данные выводятся с периодичностью где то в 2 секунды получаю не все данные... как-будто притормаживает, не успевает в порт отправить полностью. Пробовал на разных скоростях проблема та-же. И непонятно, в arduino`е всё же дело или скетч кривой?

#include <Wire.h>

unsigned long Now = millis();
unsigned long lastread = Now;
unsigned long lastreadMP = Now;

boolean debug = true;

boolean calibrating = false;

uint8_t data[6];		// array to store arduino output
int cnt = 0;
int ledPin = 13;
int xID;

static const int wmpSlowToDegreePerSec = 16; //when gyro speed 1, 16 units = 1°/s
static const int wmpFastToDegreePerSec = 4; //when gyro speed 0, 4 units = 0,25°/s

static const int NC = 0x00;
static const int MP = 0x02;
int currentDevice;

// WM+ state variables - if true, then in slow (high res) mode
boolean slowYaw = 0;
boolean slowPitch = 0;
boolean slowRoll = 0;

int rollScale;
int pitchScale;
int yawScale;

// RK integration not currently in use
struct RungeKutta {
 double val_i_3;
 double val_i_2;
 double val_i_1;
 double previous;
};

struct RungeKutta rollRK;
double roll_intRK;

// Calibrated gyro values (- yaw0 etc)
int yaw = 0;
int pitch = 0;
int roll = 0;
float rollf = 0;

float roll_int = 0;
float roll_int_rad = 0;

float dt; // msec
float dtMP; // this is the time elapsed between readings of the motion plus (MP)

float rollCF = 0;
float pitchCF = 0;

static const double DegreeToRadian = 0.0174532925;  // PI/180

static const float tau = 0.95; // unit: seconds.
float compFilter(float prevValue, int gyro, double accel, float timeStep) {
 float a = tau/(tau + dtMP);
 float b = 1 - a;
 return (float) (a*(prevValue + ((gyro*DegreeToRadian)*timeStep)) + (b*accel));
}

// accelerometer values
int ax_m = 0;
int ay_m = 0;
int az_m = 0;

// Exact values differ between x and y axis, and probably between nunchuks, but the range value may be constant:
static const int axMIN = 290;
static const int axMAX = 720;
static const int axMID = (axMAX - axMIN)/2 + axMIN;
static const int axRange = (axMID - axMIN)*2;

static const int ayMIN = 298;
static const int ayMAX = 728;
static const int ayMID = (ayMAX - ayMIN)/2 + ayMIN;
static const int ayRange = (ayMID - ayMIN)*2;

// Not sure what a meaningful scale is for the z accelerometer axis so:

static const int azMID = ayMID;
static const int azRange = ayRange;

// raw 3 axis gyro readings MP+ //gyro readings => yaw/yawscale
int readingsX;
int readingsY;
int readingsZ;

// Nunchuck variables //NunChuck X angle NOT readings
float accelAngleX=0;	//NunChuck X angle
float accelAngleY=0;	//NunChuck Y angle
float accelAngleZ=0;	//NunChuck Z angle
float theta=0;  

int z_button = 0;
int c_button = 0;
int joy_x_axis = 0;
int joy_y_axis = 0;

// calibration zeroes
//gyros
int yaw0 = 0;
int pitch0 = 0;
int roll0 = 0;

//accelerometers
int yawAcc0 = 0;
int pitchAcc0 = 0;
int rollAcc0 = 0;

double x;
double y;
double z;


//Nunchuk accelerometer value to radian conversion.
float angleInRadians(int range, int measured) {
 float x = range;
 return (float)(measured/x) * PI;
}

void
setup ()
{
 Serial.begin (115200);
 Wire.begin();

 initializeSensors();
 calibrateZeroes();

 rollRK.val_i_1 = 0;
 rollRK.val_i_2 = 0;
 rollRK.val_i_3 = 0;
 rollRK.previous = 0;

}

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

void loop ()
{
 Now = millis();
 dt = Now - lastread; //compute the time delta since last iteration, in msec.
 if (dt >= 10) //Only process delta angles if at least 1/100 of a second has elapsed. NB: 9 ms is the lowest possible interval before things jam up on an ATmega 328
 {
    readData();
    lastread = Now;
 }
 else // just for demo of the free cycles
 {
 //delay(10);
 //Serial.println("Free cycle");
 }
}

void readData()
{  
 long startReading = millis();
 //digitalWrite(13,HIGH);  // first flash the LED to show activity

   send_zero (); // send the request for next bytes
   //delay (10);

   // now follows conversion command instructing extension controller to get
   // all sensor data and put them into 6 byte register within the extension controller
   Wire.requestFrom (0x52,6);
   data[0] = Wire.receive();
   data[1] = Wire.receive();
   data[2] = Wire.receive();
   data[3] = Wire.receive();
   data[4] = Wire.receive();
   data[5] = Wire.receive();


   parseData();
 if(false)
 {
   long endReading = millis() - startReading;
  // Serial.print("Finished Reading in ");
  // Serial.print(endReading);
  // Serial.println(" milliseconds");
 }
 // digitalWrite(13,LOW);  // first flash the LED to show activity

}

void parseData ()
{
 // check if nunchuk is really connected
 if ((data[4]&0x01)==0x01) {
 //printLine("Ext con: ");

 currentDevice = data[5]&0x03;
 }
   if (currentDevice == NC)
   {

 // Dimitris version of accl readings
     ax_m = (data[2] << 2) + ((data[5] >> 3) & 2) ;
     ay_m = (data[3] << 2) + ((data[5] >> 4) & 2);
     az_m = (data[4] << 2) + ((data[5] >> 5) & 6) ;

       //Zero the values on a#MID.
       ax_m -= axMID;
       ay_m -= ayMID;
       az_m -= azMID;



// Convert to radians by mapping 180 deg of rotation to PI
       x = angleInRadians(axRange, ax_m);
       y = angleInRadians(ayRange, ay_m);
       z = angleInRadians(azRange, az_m);

  }
 else
 if  (currentDevice == MP)
 {
   dtMP = (Now - lastreadMP)/1000.0; //compute the time delta since last MP read, in sec.
   lastreadMP = Now;

     // duck's hardware setup for this code has the MP spun 90 deg relative to the NC, so I swapped the roll and pitch to match a setup with the NC and MP on the same axis
     // Gyroscopes: dual-axis IDG-600, see http://www.invensense.com/products/idg_600.html and EPSON TOYOCOM labelled X3500W (yaw)
   roll = (((data[4]&0xFC)<<6) + data[1]);
   pitch = (((data[5]&0xFC)<<6) + data[0]);
   yaw = (((data[3]&0xFC)<<6) + data[2]);

   if(calibrating == false) //Dont compensate if we are doing the calibration measurements.
   {

     slowPitch = (data[4]&0x02)>>1;
     slowRoll = (data[3]&0x01)>>0;
     slowYaw = (data[3]&0x02)>>1;

     rollScale  = slowRoll  ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec; // if speed == 1, then wmpSlowToDegreePerSec, else wmpFastToDegreePerSec
     pitchScale = slowPitch ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
     yawScale   = slowYaw   ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;

     //Calibrating
     roll -= roll0;
     pitch -= pitch0;
     yaw -= yaw0;

     roll = roll/rollScale;    // deg/s = mV/(mV/deg/s)
     pitch = pitch/pitchScale;
     yaw = yaw/yawScale;

     // Complimentary filter: inspired by Shane Colton's description in filter.pdf at http://web.mit.edu/first/segway/segspecs.zip
       rollCF = compFilter(rollCF, roll, x, dtMP);
       pitchCF = compFilter(pitchCF, pitch, y, dtMP);

   }

 }


 if(debug)
 {
    //Serial.print ("y");
 Serial.println (x*57.295779513,0);      
 //Serial.print ("y");
 Serial.println (y*57.295779513,0);
   //Serial.print ("z");
   //Serial.print (z);
   // Serial.print ("rollCF ");

   //Serial.println (rollCF);
  // Serial.print ("     ");
   //Serial.println (pitchCF);
  //Serial.print ("roll");
  Serial.println (roll);  
  //Serial.print ("pitch");
  Serial.println (pitch);  
  //Serial.print ("yaw");
  Serial.print (yaw);

delay(60);
 }
}

void initializeSensors()
{

 digitalWrite(13,HIGH);  // first flash the LED to show activity
 //Serial.println ("Now detecting WM+");
 //delay(100);
 // now make Wii Motion plus the active extension
 // nunchuk mode = 05, wm+ only = 04, classic controller = 07
 //Serial.println ("Activating WM+ in NC mode (5)...");
  Wire.beginTransmission(0x53);
  Wire.send(0xFE);
  Wire.send(0x05);// nunchuk
  Wire.endTransmission();
 //Serial.println (" OK done");
 // delay (100);
 // now innitiate Wii Motion plus
  //Serial.println ("Innitialising Wii Motion plus ........");
   Wire.beginTransmission(0x53);
   Wire.send(0xF0);
   Wire.send(0x55);
   Wire.endTransmission();
// Serial.println(" OK done");
  // delay (100);
 //Serial.println ("Reading address at 0xFA .......");
   Wire.beginTransmission(0x52);
   Wire.send(0xFA);
   Wire.endTransmission();
// Serial.println(" OK done");
 // delay (100);
  Wire.requestFrom (0x52,6);
  data[0] = Wire.receive();//Serial.print(outbuf[0],HEX);Serial.print(" ");
  data[1] = Wire.receive();//Serial.print(outbuf[1],HEX);Serial.print(" ");
  data[2] = Wire.receive();//Serial.print(outbuf[2],HEX);Serial.print(" ");
  data[3] = Wire.receive();//Serial.print(outbuf[3],HEX);Serial.print(" ");
  data[4] = Wire.receive();//Serial.print(outbuf[4],HEX);Serial.print(" ");
  data[5] = Wire.receive();//Serial.print(outbuf[5],HEX);Serial.print(" ");

  xID= data[0] + data[1] + data[2] + data[3] + data[4] + data[5];
// Serial.println("Extension controller xID = 0x");
 // Serial.print(xID,HEX);
  //if (xID == 0xCB) { Serial.println ("MP+ on but not active"); }
  //if (xID == 0xCE) { Serial.println ("MP+ on and active"); }
  //if (xID == 0x00) { Serial.println ("MP+ not on"); }
  //delay (100);
  // Now we want to point the read adress to 0xa40008 where the 6 byte data is stored
// Serial.println ("Reading address at 0x08 .........");
   Wire.beginTransmission(0x52);
   Wire.send(0x08);
   Wire.endTransmission();
// Serial.println(" OK done");
 //Serial.println ("Finished setup");

 digitalWrite(13,LOW); // Led off
}

/*
* Find the steady-state of the WM+ gyros.  This function reads the gyro values 100 times and averages them.
* Those values are used as the baseline steady-state for all subsequent reads.  As a result it is very
* important that the device remain relatively still during startup.
*/


void calibrateZeroes(){  // wiibrew: While the Wiimote is still, the values will be about 0x1F7F (8,063)
 calibrating = true;
 if(debug)
 {
   //Serial.println("Starting calibration...");
 }
 //Gyros
 long y0 = 0;
 long p0 = 0;
 long r0 = 0;

 //Accelerometers for application calibration, not for zeroing the starting points.
 long ya0 = 0;
 long pa0 = 0;
 long ra0 = 0;

 const int avg = 200; //100 reads of the gyros and 100 of the accelerometers

 digitalWrite(13,HIGH);  // first flash the LED to show activity

 for (int i=0;i<avg; i++)
 {
   delay(10); // bypasses the normal global delays
   readData();

   if(currentDevice == MP)
   {
     if(debug)
     {
      // Serial.println("<= Data from MP");
     }

     y0+=yaw;
     r0+=roll;
     p0+=pitch;
   }
   else
   {
      if(debug)
     {
     //  Serial.println("<= Data from NC...");
     }
     ya0 += az_m;
     pa0 += ay_m;
     ra0 += ax_m;

   }
 }

 yaw0 = y0/(avg/2);
 roll0 = r0/(avg/2);
 pitch0 = p0/(avg/2);

 yawAcc0 = ya0/(avg/2);
 rollAcc0 = ra0/(avg/2);
 pitchAcc0 = pa0/(avg/2);

 calibrating = false;

 digitalWrite(13,LOW); // Led off
 if(debug)
 {
   //Serial.println("Calibration results...");
  // Serial.print("Yaw0:");
  // Serial.println(yaw0);
  // Serial.print("Roll0:");
  // Serial.println(roll0);
   //Serial.print("Pitch0:");
  // Serial.println(pitch0);
  // Serial.print("YawAcc0:");
  // Serial.println(yawAcc0);
  // Serial.print("RollAcc0:");
  // Serial.println(rollAcc0);
  // Serial.print("PitchAcc0:");
  // Serial.println(pitchAcc0);
 }
}

double computeRK4(struct RungeKutta *rk, double val_i_0) {
 rk->previous += 0.16667*(rk->val_i_3 + 2*rk->val_i_2 + 2*rk->val_i_1 + val_i_0);
 rk->val_i_3 = rk->val_i_2;
 rk->val_i_2 = rk->val_i_1;
 rk->val_i_1 = val_i_0;
 return rk->previous;
}