Pages: 1 [2] 3 4 ... 12   Go Down
Author Topic: Kalman Filtered Nunchuck & Wii Motion Plus  (Read 25592 times)
0 Members and 1 Guest are viewing this topic.
0
Offline Offline
Full Member
***
Karma: 0
Posts: 113
Arduino paper
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Checkout http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus if you haven't.

In 'slow' mode, 20 represents turning at about 1 degree per second. So you divide by 20 to get the degrees per second. At high speed (Low speed bit = 0) 20 represents turning at about 5 degree per second, So you have to divide by 4 to get the degrees per second.

You will also see in the Data Format table that Byte 3 and 4, bits 0 and 1 contain flags for what mode it is in.

Thats why my original code is off - It only calculates in the 'slow mode' 20/sec mode correctly and doesn't look at the mode-bits to compensate...
« Last Edit: August 04, 2009, 10:14:01 pm by adr1an » Logged

Checkout my projects development blog @ SLiDA

0
Offline Offline
Newbie
*
Karma: 0
Posts: 31
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Ahh...got it ! and I meant 14 bits from the a/d. It all makes sense now. Hope your coming along on the code. A lot of autopilots and other things sure can use an inexpensive GOOD 6DOF. I think your in the right track incorporating the kalman filter. That is going to be the secret in making this work as a 6DOF. Good work.
Earl
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 21
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Yes, that's right.  Here's a code snippet of what this would look like:
Code:
bool slowYaw;
bool slowPitch;
bool slowRoll;

const int wmpSlowToDegreePerSec = 20;
const int wmpFastToDegreePerSec = 4;


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];
  roll=((data[4] >> 2) << 8)+data[1];
  pitch=((data[5] >> 2) << 8)+data[2];

  slowPitch = data[3] & 1;
  slowYaw = data[3] & 2;
  slowRoll = data[4] & 2;
}


void loop() {
  ....

  const int rollScale  = slowRoll  ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
  const int pitchScale = slowPitch ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
  const int yawScale   = slowYaw   ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
    
  readingsX[index] = int(roll/rollScale);  
  readingsY[index] = int(pitch/pitchScale);
  readingsZ[index] = int(yaw/yawScale);
 ....        
}

Duck
Logged

Florida
Offline Offline
Full Member
***
Karma: 0
Posts: 171
1 Cor 10:31
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Has anyone actually confirmed what wiibrew says about fast and slow divisions to be true. I had originally posted on that site that the division factor for the yaw axis only was 5 on fast mode. Since then it seems to have been changed to what it now says. I am running a similar code to what duckhead just posted through simple integration program and pitch and roll axes do not seem to be agreeing with wiibrew. Only yaw works as it should with the division factor of 4. Remember that wiibrew is a wiki so take it for what its worth.

Can anyone confirm correct working of the 3 axes in fast mode?
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 31
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Knuckles,
Can you post the latest code your playing with ?
Earl
Logged

Florida
Offline Offline
Full Member
***
Karma: 0
Posts: 171
1 Cor 10:31
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Sure, here's the integration sketch i have been using. Its also configured so that it can be used with the rgb cube tester with minimal modifications for visual. But using this you can see that while turning the wm+ quickly, only one axis gives valid angular position.
Code:
//Trapezoid or Runge-kutta 4th order integration program of wm+

#include <Streaming.h>
#include <Wire.h>
//#include <WiiChuck.h>

#define steps_per_deg_slow 20
#define steps_per_deg_fast 4

byte data[6];          //six data bytes
int yaw, pitch, roll;  //three axes
int yaw0, pitch0, roll0;  //calibration zeroes
int time, last_time;
float delta_t;
int last_yaw[3], last_pitch[3], last_roll[3];
float yaw_deg, pitch_deg, roll_deg;
int yaw_deg2, pitch_deg2, roll_deg2;
int startTag=0xDEAD;
int accel_x_axis, accel_y_axis, accel_z_axis;

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 for each zero
    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();
  }
  if(bitRead(data[3], 1)==1) yaw=(((data[3]>>2)<<8)+data[0]-yaw0)/steps_per_deg_slow;        //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
  else yaw=(((data[3]>>2)<<8)+data[0]-yaw0)/steps_per_deg_fast;
  if(bitRead(data[3], 0)==1) pitch=(((data[4]>>2)<<8)+data[1]-pitch0)/steps_per_deg_slow;    //for info on what each byte represents
  else pitch=(((data[4]>>2)<<8)+data[1]-pitch0)/steps_per_deg_fast;
  if(bitRead(data[4], 1)==1) roll=(((data[5]>>2)<<8)+data[2]-roll0)/steps_per_deg_slow;
  else roll=(((data[5]>>2)<<8)+data[2]-roll0)/steps_per_deg_fast;
}

void setup(){
  Serial.begin(115200);
  Serial.println("WM+ tester");
  pinMode(3, OUTPUT);
  digitalWrite(3, HIGH);
  delay(1);
  Wire.begin();
  wmpOn();                        //turn WM+ on
  calibrateZeroes();              //calibrate zeroes
  delay(1000);
}

void loop(){
  receiveData();                  //receive data and calculate yaw pitch and roll
  time=millis();
  delta_t=(time-last_time);
  /*yaw_deg+=rk4Integrate(yaw, last_yaw[0], last_yaw[1], last_yaw[2], delta_t);
  pitch_deg+=rk4Integrate(pitch, last_pitch[0], last_pitch[1], last_pitch[2], delta_t);
  roll_deg+=rk4Integrate(roll, last_roll[0], last_roll[1], last_roll[2], delta_t);*/
  yaw_deg+=trapIntegrate(yaw, last_yaw[0], delta_t);
  pitch_deg+=trapIntegrate(pitch, last_pitch[0], delta_t);
  roll_deg+=trapIntegrate(roll, last_roll[0], delta_t);
  /*last_yaw[2]=last_yaw[1];
  last_pitch[2]=last_pitch[1];
  last_roll[2]=last_roll[1];
  last_yaw[1]=last_yaw[0];
  last_pitch[1]=last_pitch[0];
  last_roll[1]=last_roll[0];*/
  last_yaw[0]=yaw;
  last_pitch[0]=pitch;
  last_roll[0]=roll;
  last_time=time;
  
  yaw_deg2=yaw_deg;//+360;
  pitch_deg2=pitch_deg;//+360;
  roll_deg2=roll_deg;//+360;
  
  Serial<<yaw_deg2<<"\t"<<pitch_deg2<<"\t"<<roll_deg2<<"\n";
  /*
  Serial.write((unsigned byte*)&startTag, 2);
  Serial.write((unsigned byte*)&accel_x_axis, 2);    
  Serial.write((unsigned byte*)&accel_y_axis, 2);
  Serial.write((unsigned byte*)&accel_z_axis, 2);
  Serial.write((unsigned byte*)&pitch_deg2, 2);
  Serial.write((unsigned byte*)&yaw_deg2, 2);
  Serial.write((unsigned byte*)&roll_deg2, 2);*/
  delay(10);
}

float trapIntegrate(int y2, int y1, float deltax){
  float area=0;
  area=(y2+y1)/2*deltax/1000;
  return area;
}

float rk4Integrate(int y4, int y3, int y2, int y1, float deltax){
  float area=0;
  area=((y4+2*y3+2*y2+y1)/6)*deltax/1000;
  return area;
}
As for the kalman sketch im working on, its going really slowly. No real kalman filtering stuff as of now cuz im still trying to wrap my head around it-I like jordi's code but its worthless to me unless I understand what everything does. Ill have to be the one to tune and problem solve so I need to fully understand it. Right now im working on that aspect. Luckily there are lots of great posts like tom pycke's but unluckily, very few give the right amount of physical examples and (lack of) abstraction for me to figure it all out.
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 31
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

OK thanks. The thing I noticed is if you turn on or reset the arduino the wmp numbers are about 8000 on yaw pitch and roll. So far so good.
Then if you don't touch anything, the numbers keep increasing and increasing at a rate of about 4 counts per second. If I physically move the wmp, the numbers don't change that much, and then they go back to increasing at a rate of about 4 counts per second. Is that what yours does?
I have another program that seams to work properly almost. So I don't suspect the wmp itself. I tried a second wmp and the same behavior.

Hope this helps your work.
Earl
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 31
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Ahh... I think I found my problem. I was just using the WMP alone and not the WMP/NC with the transistor switches. I'll hook that circuit back up and see what I get.
Earl
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 31
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Yaw seams to work ok. That was the problem. I now am on with the WMP/NC combo.

That's strange. Having just the WMP should work fine also because we aren't getting any data from the NC.
Must be because there is a state digital port 3 and 4 both low. But if it isn't connected to anything it shouldn't matter.

In the mode with the WMP always connected, the data gets screwed up.
Mmmm......
Earl

Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 21
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Here's my latest.  It's seems to be working well for me, but I have more cleanup and optimizations, as well as some more thought on correlating the linear axes from the nunchuk to the rotational axes of the WMP.
Code:
// Modified Kalman code using Roll, Pitch, and Yaw from a Wii MotionPlus and X, Y, and Z accelerometers from a Nunchuck.
// Also uses "new" style Init to provide unencrypted data from the Nunchuck to avoid the XOR on each byte.
// Requires the WM+ and Nunchuck to be connected to Arduino pins D3/D4 using the schematic from Knuckles904 and johnnyonthespot
// See http://randomhacksofboredom.blogspot.com/2009/07/motion-plus-and-nunchuck-together-on.html
// Kalman Code By Tom Pycke. http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data
// Original zipped source with very instructive comments: http://tom.pycke.be/file_download/4
//
// Some of this code came via contributions or influences by Ed Simmons, Jordi Munoz, and Adrian Carter
//
// Created by Duckhead   v0.2   some cleanup and other optimization work remains.  also need to investigate correlation of nunchuk and WMP (which axis is which)

#include <math.h>
#include <Wire.h>
/////////////////////////////////////////

struct GyroKalman
{
  /* These variables represent our state matrix x */
  float x_angle,
        x_bias;

  /* Our error covariance matrix */
  float P_00,
        P_01,
        P_10,
        P_11;      

  /*
   * Q is a 2x2 matrix of the covariance. Because we
   * assume the gyro and accelerometer noise to be independent
   * of each other, the covariances on the / diagonal are 0.
   *
   * Covariance Q, the process noise, from the assumption
   *    x = F x + B u + w
   * with w having a normal distribution with covariance Q.
   * (covariance = E[ (X - E[X])*(X - E[X])' ]
   * We assume is linear with dt
   */
  float Q_angle, Q_gyro;

  /*
   * Covariance R, our observation noise (from the accelerometer)
   * Also assumed to be linair with dt
   */
  float R_angle;
};


struct GyroKalman rollData;
struct GyroKalman pitchData;
struct GyroKalman yawData;

// test for 3 axis
int readingsX;
int readingsY;
int readingsZ;                // the readings
// End of 3 axis stuff
int index = 0;                            // the index of the current reading
int inputPin =0;                          // Gyro Analog input
//WM+ stuff
byte data[6]; //six data bytes
int yaw = 0;
int pitch = 0;
int roll = 0; //three axes
int yaw0 = 0;
int pitch0 = 0;
int roll0 = 0; //calibration zeroes
bool slowYaw;
bool slowPitch;
bool slowRoll;

const int wmpSlowToDegreePerSec = 20;
const int wmpFastToDegreePerSec = 4;

///////////////////////////////////////

float accelAngleX=0;      //NunChuck X angle
float accelAngleY=0;      //NunChuck Y angle
float accelAngleZ=0;      //NunChuck Z angle
byte outbuf[6];            // array to store arduino output
int cnt = 0;            //Counter
unsigned long lastread=0;

/*
 * R represents the measurement covariance noise.  In this case,
 * it is a 1x1 matrix that says that we expect 0.3 rad jitter
 * from the accelerometer.
 */
static const float      R_angle      = .3; //.3 default

/*
 * Q is a 2x2 matrix that represents the process covariance noise.
 * In this case, it indicates how much we trust the acceleromter
 * relative to the gyros.
 */
static const float      Q_angle      = 0.01; //(Kalman)
static const float      Q_gyro      = 0.04; //(Kalman)

//These are the limits of the values I got out of the Nunchuk accelerometers (yours may vary).
const int lowX = -215;
const int highX = 221;

const int lowY = -215;
const int highY = 221;

const int lowZ = -215;
const int highZ = 255;

/*
 * Nunchuk accelerometer value to degree conversion.  Output is -90 to +90 (180 degrees of motion).  
 */
float angleInDegrees(int lo, int hi, int measured) {
  float x = (hi - lo)/180.0;
  return (float)measured/x;
}

void switchtonunchuck(){
/*  PORTE ^= 32; // Toggle D3 LOW
  PORTG |= 32; // Force D4 HIGH */ // Fast switching of pins... Arduino MEGA specific...
  digitalWrite(3, LOW);
  digitalWrite(4, LOW);
  digitalWrite(4, HIGH);

}

void switchtowmp(){
/*  PORTG ^= 32; // Toggle D4 LOW
  PORTE |= 32; // Force D3 HIGH */ // Fast switching of pins... Arduino MEGA Specific pin maps, so switched back to 'normal' code for example
  digitalWrite(3, LOW);
  digitalWrite(4, LOW);
  digitalWrite(3, HIGH);
}

void nunchuck_init () //(nunchuck)
{
  // Uses New style init - no longer encrypted so no need to XOR bytes later... might save some cycles
  Wire.beginTransmission (0x52);      // transmit to device 0x52
  Wire.send (0xF0);            // sends memory address
  Wire.send (0x55);            // sends sent a zero.
  Wire.endTransmission ();      // stop transmitting
  delay(100);
  Wire.beginTransmission (0x52);      // transmit to device 0x52
  Wire.send (0xFB);            // sends memory address
  Wire.send (0x00);            // sends sent a zero.
  Wire.endTransmission ();      // stop transmitting
}

void setup()
{
  Serial.begin(115200);
/*  DDRG |= 32; // Force D3 to Output // Arduino MEGA "fast" pin config. Reverted to 'normal' for example post.
  DDRE |= 32; // Force D4 to Output
  PORTG ^= 32; // Toggle D3 HIGH
  PORTE ^= 32; // Toggle D4 HIGH */

  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  digitalWrite(3, HIGH);
  digitalWrite(4, HIGH);

  Wire.begin ();            // join i2c bus with address 0x52 (nunchuck)
  switchtowmp();
  wmpOn();
  calibrateZeroes();
  switchtonunchuck();
  nunchuck_init (); // send the initilization handshake

  initGyroKalman(&rollData, Q_angle, Q_gyro, R_angle);
  initGyroKalman(&pitchData, Q_angle, Q_gyro, R_angle);
  initGyroKalman(&yawData, Q_angle, Q_gyro, R_angle);
  
  lastread = millis();
}

void initGyroKalman(struct GyroKalman *kalman, const float Q_angle, const float Q_gyro, const float R_angle) {
  kalman->Q_angle = Q_angle;
  kalman->Q_gyro  = Q_gyro;
  kalman->R_angle = R_angle;
  
  kalman->P_00 = 0;
  kalman->P_01 = 0;
  kalman->P_10 = 0;
  kalman->P_11 = 0;
}

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

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.endTransmission();
}

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

void calibrateZeroes(){
  long y0 = 0;
  long p0 = 0;
  long r0 = 0;
  const int avg = 10;
  for (int i=0;i<avg; i++){
    wmpSendZero();
    Wire.requestFrom(0x52,6);
    for (int t=0;t<6;t++){
      data[t]=Wire.receive();
    }
    y0+=(((data[3] >> 2) << 8)+data[0]);
    r0+=(((data[4] >> 2) << 8)+data[1]);
    p0+=(((data[5] >> 2) << 8)+data[2]);
  }
  
  yaw0 = y0/avg;
  roll0 = r0/avg;
  pitch0 = p0/avg;
}

void receiveData(){
  receiveRaw();
  //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
  //for info on what each byte represents
  yaw -= yaw0;
  roll -= roll0;
  pitch -= -pitch0;
}

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];
  roll=((data[4] >> 2) << 8)+data[1];
  pitch=((data[5] >> 2) << 8)+data[2];
  
  slowPitch = data[3] & 1;
  slowYaw = data[3] & 2;
  slowRoll = data[4] & 2;
}

void loop()
{
  unsigned long now = millis();
  float dt = ((float)(now - lastread))/1000.0; //compute the time delta since last iteration, in seconds.

  if (dt >= 0.01) {  //Only process delta angles if at least 1/100 of a second has elapsed
    lastread = now;

    switchtowmp();
    receiveData();
    switchtonunchuck();
    readNunchuck();

    // Handle the fast/slow bits of the Wii MotionPlus
    const int rollScale  = slowRoll  ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
    const int pitchScale = slowPitch ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
    const int yawScale   = slowYaw   ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
    
    readingsX = (roll/rollScale); // read from the gyro sensor
    readingsY = (pitch/pitchScale); // read from the gyro sensor
    readingsZ = (yaw/yawScale); // read from the gyro sensor
    
    float tmpX = accelAngleX * (PI/180.0);
    float tmpY = accelAngleY * (PI/180.0);
    float tmpZ = accelAngleZ * (PI/180.0);

    predict(&rollData, readingsX, dt);
    predict(&pitchData, readingsY, dt);
    predict(&yawData, readingsZ, dt);
  
    float rollAngle = update(&rollData, tmpX);
    float pitchAngle = update(&pitchData, tmpY);
    float yawAngle = update(&yawData, tmpZ);

    Serial.print("Roll: ");
    Serial.print(rollAngle*(180.0/PI));
    Serial.print("Pitch: ");
    Serial.print(pitchAngle*(180.0/PI));
    Serial.print("Yaw: ");
    Serial.println(yawAngle*(180.0/PI));
  }
}

//Stitch Here

Duck
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 21
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Continuation:
Code:
//Stitch here
/*
 * The kalman predict method.  See http://en.wikipedia.org/wiki/Kalman_filter#Predict
 *
 * kalman    the kalman data structure
 * dotAngle  Derivitive Of The (D O T) Angle.  This is the change in the angle from the gyro.  This is the value from the
 *           Wii MotionPlus, scaled to fast/slow.
 * dt        the change in time, in seconds; in other words the amount of time it took to sweep dotAngle
 *
 * Note: Tom Pycke's ars.c code was the direct inspiration for this.  However, his implementation of this method was inconsistent
 *       with the matrix algebra that it came from.  So I went with the matrix algebra and tweaked his implementation here.
 */
void predict(struct GyroKalman *kalman, float dotAngle, float dt) {

  kalman->x_angle += dt * (dotAngle - kalman->x_bias);
  kalman->P_00 += -1 * dt * (kalman->P_10 + kalman->P_01) + dt*dt * kalman->P_11 + kalman->Q_angle;
  kalman->P_01 += -1 * dt * kalman->P_11;
  kalman->P_10 += -1 * dt * kalman->P_11;
  kalman->P_11 += kalman->Q_gyro;
}

/*
 * The kalman update method.  See http://en.wikipedia.org/wiki/Kalman_filter#Update
 *
 * kalman    the kalman data structure
 * angle_m   the angle observed from the Wii Nunchuk accelerometer, in radians
 */
float update(struct GyroKalman *kalman, float angle_m) {
  const float y = angle_m - kalman->x_angle;
  const float S = kalman->P_00 + kalman->R_angle;
  const float K_0 = kalman->P_00 / S;
  const float K_1 = kalman->P_10 / S;

  kalman->x_angle += K_0 * y;
  kalman->x_bias  += K_1 * y;
  
  kalman->P_00 -= K_0 * kalman->P_00;
  kalman->P_01 -= K_0 * kalman->P_01;
  kalman->P_10 -= K_1 * kalman->P_00;
  kalman->P_11 -= K_1 * kalman->P_01;
  
  return kalman->x_angle;
}

void readNunchuck () {
    Wire.requestFrom (0x52, 6);      // request data from nunchuck

    while (Wire.available ()) //(NunChuck)
    {
      outbuf[cnt] = Wire.receive ();      // receive byte as an integer //(NunChuck)
      cnt++;//(NunChuck)
    }

    send_zero (); // send the request for next bytes
    // If we recieved the 6 bytes, then print them //(NunChuck)
    if (cnt >= 5) //(NunChuck)
    {
      readAccelerometers (); //(NunChuck)
    }
    cnt = 0; //(NunChuck)
}

void readAccelerometers ()//This is the function to get the bytes from nintendo wii nunchuck
{
  int ax_m = (outbuf[2] * 2 * 2); //Axis X Accelerometer.  Shift 2 bits to the left.
  int ay_m = (outbuf[3] * 2 * 2); //Axis Y Accelerometer.  Shift 2 bits to the left.
  int az_m = (outbuf[4] * 2 * 2); //Axis Z Accelerometer.  Shift 2 bits to the left.

  // 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] >> 2) & 1)
  {
    ax_m += 2;
  }
  if ((outbuf[5] >> 3) & 1)
  {
    ax_m += 1;
  }

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

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

  //The nunchuk accelerometers read ~511 steady state.  Remove that to zero the values.
  ax_m -= 511;
  ay_m -= 511;
  az_m -= 511;
    
  //Translate the raw accelerometer counts into corresponding degrees
  accelAngleZ = angleInDegrees(lowZ, highZ, az_m);
  accelAngleY = angleInDegrees(lowY, highY, ay_m);
  accelAngleX = angleInDegrees(lowX, highX, ax_m);
}

Logged

0
Offline Offline
Full Member
***
Karma: 0
Posts: 113
Arduino paper
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

cool - i'll have a fiddle shortly, just back from my local chip pimp with some new supplies...
Logged

Checkout my projects development blog @ SLiDA

0
Offline Offline
Newbie
*
Karma: 0
Posts: 21
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Sorry.  Posted the wrong loop() method in my earlier post.  Use this one instead (the difference is that the gyro derivative of the angle needs to be in radians).
Code:
void loop()
{
  unsigned long now = millis();
  float dt = ((float)(now - lastread))/1000.0; //compute the time delta since last iteration, in seconds.

  if (dt >= 0.01) {  //Only process delta angles if at least 1/100 of a second has elapsed
    lastread = now;

    switchtowmp();
    receiveData();
    switchtonunchuck();
    readNunchuck();

    // Handle the fast/slow bits of the Wii MotionPlus
    const int rollScale  = slowRoll  ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
    const int pitchScale = slowPitch ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
    const int yawScale   = slowYaw   ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
    
    readingsX = (roll/rollScale); // read from the gyro sensor
    readingsY = (pitch/pitchScale); // read from the gyro sensor
    readingsZ = (yaw/yawScale); // read from the gyro sensor
    
    float tmpX = accelAngleX * (PI/180.0);
    float tmpY = accelAngleY * (PI/180.0);
    float tmpZ = accelAngleZ * (PI/180.0);

    predict(&rollData, readingsX * (PI/180.0), dt);
    predict(&pitchData, readingsY * (PI/180.0), dt);
    predict(&yawData, readingsZ * (PI/180.0), dt);
  
    float rollAngle = update(&rollData, tmpX);
    float pitchAngle = update(&pitchData, tmpY);
    float yawAngle = update(&yawData, tmpZ);

    Serial.print("Roll: ");
    Serial.print(rollAngle*(180.0/PI));
    Serial.print("Pitch: ");
    Serial.print(pitchAngle*(180.0/PI));
    Serial.print("Yaw: ");
    Serial.println(yawAngle*(180.0/PI));
  }
}

Duck
Logged

0
Offline Offline
Full Member
***
Karma: 0
Posts: 113
Arduino paper
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

BTW.. for anyone looking for some help on what all the numbers mean.. heres some good ideas (in terms of what some of the constants in the kalman calcs are etc etc). Its for a straight complementary filter, but it shows the radians and angle calcs etc.

http://www.ohscope.com/
and http://www.mikroquad.com/bin/view/Research/ComplementaryFilter
« Last Edit: August 06, 2009, 12:52:45 am by adr1an » Logged

Checkout my projects development blog @ SLiDA

Florida
Offline Offline
Full Member
***
Karma: 0
Posts: 171
1 Cor 10:31
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Looks absolutely great. Beautiful even. Thats exactly what I want my code to look like structure wise. (I was planning on wrapping it all in a class) Very efficient. I do have one problem with it however. You didnt use any trig functions to process accelerometer data. I see that your way does work very well for some things but it makes it much more susceptible to high error values upon vibration and reduces resolution of the accelerometer by a full 180 degrees on pitch and roll as well as makes yaw pretty much useless(though yes it is useless and erratic around horizontal). I switched your last three lines of code to:
Code:
//Translate the raw accelerometer counts into corresponding degrees
  accelAngleZ = atan2(ax_m, ay_m)*180/PI;
  accelAngleY = atan2(ay_m, az_m)*180/PI;
  accelAngleX = atan2(ax_m, az_m)*180/PI;
}
Let me know if you see a reason I and others shouldnt
Logged

Pages: 1 [2] 3 4 ... 12   Go Up
Jump to: