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

This is my first post of some of my code, so please be gentle. But *please please* help me optimise it!!  ;D

I've been closely following Knuckles904's work getting the Wii MotionPlus working, and then in combination with the Nunchuck.
http://randomhacksofboredom.blogspot.com/2009/07/motion-plus-and-nunchuck-together-on.html

I have taken this work and combined it back into Jordi Muñoz's Kalman Filter example used in ArduPilot http://diydrones.com/profiles/blog/show?id=705844%3ABlogPost%3A23188

I have also switched the Nunchuck Init code to use the 'new' style init, which means the Nunchuck data is Non-Encrypted, avoiding the need to XOR the bytes...

The bits I am *completely* vague on - most of the Kalman stuff  ;D ;D I have tried to extend it to all 3-axis, but I think its off somewhere... I eventually would love to incorporate this with GPS data .. But first things first, I want to extend the code to work properly to make a 6DOF IMU from Nintendo bits !! So any help here would be greatly appreciated.

I know the gyro data is off in the kalman filter - obviously need to play with some numbers - but i'm not real sure on to what and where !

Here is the code

<snipped>

I've had a play, and think I have it *kind of* working with 3 axis incorporated.

Its a modification of Ed Simmons code which also shares the same roots as mine from http://www.eclipseaudioservices.co.uk/extras/helicopter.html

The first line of output - Jordi's original 1-axis kalman works fine still. But the numbers on the other axis' are clearly screwy !  :-[
Code:
// Modified Kalman code using the Y-Axis from a Wii MotionPlus and the X and Z from a Nunchuck.
// Nunchuck joystick provides corrections - X for Gyro, Y for Accelerometer
// 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
// See http://randomhacksofboredom.blogspot.com/2009/07/motion-plus-and-nunchuck-together-on.html
// Kalman Code Originally By Jordi Munoz... //Si deseas la version en Espanol, no dudes en escribirme...  
//
// Altered by Adrian Carter <adrian@canberrariders.org.au> to work with WiiM+ and Nunchuck

#include <math.h>
#include <Wire.h>
/////////////////////////////////////////
#define NUMREADINGS 5 //Gyro noise filter
// test for 3 axis
int readingsX[NUMREADINGS];
int readingsY[NUMREADINGS];
int readingsZ[NUMREADINGS];                // the readings
int totalX = 0;
int totalY = 0;
int totalZ = 0;                           // the running total
int averageX = 0;                          // the average
int averageY = 0;
int averageZ = 0;
int lastaverageZ =0;
// End of 3 axis stuff
int readings[NUMREADINGS];                // the readings from the analog input (gyro)
int index = 0;                            // the index of the current reading
int total = 0;                            // the running total
int average = 0;                          // the average
int inputPin =0;                          // Gyro Analog input
//WM+ stuff
byte data[6]; //six data bytes
int yaw, pitch, roll; //three axes
int yaw0, pitch0, roll0; //calibration zeroes
///////////////////////////////////////

float      dt      = .06; //( 1024.0 * 256.0 ) / 16000000.0; (Kalman)
int mydt = 20; //in ms.
static float            P[2][2] = { //(Kalman)

  {
    1, 0   }
  , //(Kalman)
  {
    0, 1   }
  ,//(Kalman)
}; //(Kalman)
// extra 2 axis..
static float            P1[2][2] = {  { 1, 0 } ,  { 0, 1 } ,};
static float            P2[2][2] = {  { 1, 0 } ,  { 0, 1 } ,};
static float            P3[2][2] = {  { 1, 0 } ,  { 0, 1 } ,}; //(Kalman)


/*
 * Our two states, the angle and the gyro bias.  As a byproduct of computing
 * the angle, we also have an unbiased angular rate available.   These are
 * read-only to the user of the module.
 */
float                  angle; //(Kalman)
float                   angleX;
float                   angleY;
float                   angleZ;
float                  q_bias; //(Kalman)
float                   q_bias_X;
float                   q_bias_Y;
float                   q_bias_Z;
float                   rate_X;
float                   rate_Y;
float                   rate_Z;
float                   q_m_X; //(Kalman)
float                   q_m_Y;
float                   q_m_Z;
float                   rotationZ;
float                  rate; //(Kalman)
float                   q_m; //(Kalman)

// adjusts
int gyro_adjust = 1;
int accel_adjust = 128;

int joy_x_axis = 0; //NunChuck Joysticks potentiometers
int joy_y_axis = 0;
int ax_m=0;      //NunChuck X acceleration
// Re-added Y Axis
int ay_m=0;      //NunChuck Y acceleration
int az_m=0;      //NunChuck Z acceleration
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.
 */
float      R_angle      = .3; //.3 deafault, but is adjusted externally (Kalman)


/*
 * 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.001; //(Kalman)
static const float      Q_gyro      = 0.003; //(Kalman)

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  

  for (int i = 0; i < NUMREADINGS; i++)
    readings[i] = 0;                      // initialize all the readings to 0 (gyro average filter)
}

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

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 loop()
{
  if((millis()-lastread) >= mydt) { // sample every dt ms -> 1000/dt hz.
    lastread = millis();
    total += readings[index];
    totalX += readingsX[index];
    totalY += readingsY[index];
    totalZ += readingsZ[index];               // add the reading to the total
    switchtowmp();
    receiveData();
    switchtonunchuck();
    readings[index] = int(pitch/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
    readingsX[index] = int(yaw/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
//Continued
« Last Edit: July 29, 2009, 03:54:34 pm by adr1an » Logged

Checkout my projects development blog @ SLiDA

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

Code:
   readingsX[index] = int(roll/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
    readingsY[index] = int(pitch/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
    readingsZ[index] = int(yaw/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
//    receiveRaw(); // Provides unbiased output from WM+ (i.e, without calibrationZeros)
    

    index = (index + 1);                    // advance to the next index

    if (index >= NUMREADINGS)               // if we're at the end of the array...
      index = 0;                            // ...wrap around to the beginning

    average = (total / NUMREADINGS) - 511;
    averageX = (totalX / NUMREADINGS) - 511;
    averageY = (totalY / NUMREADINGS) - 511;
    averageZ = (totalZ / NUMREADINGS) - 511;    // calculate the average of gyro input

    q_m=((average*(joy_x_axis*0.00392156862))/(180/PI)); //GYRO convertion to Radian and external correction with nunchuk joystick
    q_m_X=((averageX*(joy_x_axis*0.00392156862))/(180/PI)); //GYRO convertion to Radian and external correction with nunchuk joystick
    q_m_Y=((averageY*(joy_x_axis*0.00392156862))/(180/PI)); //GYRO convertion to Radian and external correction with nunchuk joystick
    q_m_Z=((averageZ*(joy_x_axis*0.00392156862))/(180/PI)); //GYRO convertion to Radian and external correction with nunchuk joystick

    /* Unbias our gyro */
    const float      q_X = q_m_X - q_bias_X; //(Kalman)
    const float q_Y = q_m_Y - q_bias_Y;
    const float q_Z = q_m_Z - q_bias_Z;
    const float      q = q_m - q_bias; //(Kalman)

    const float            Pdot[2 * 2] = {
      Q_angle - P[0][1] - P[1][0],      /* 0,0 */ //(Kalman)
      - P[1][1],            /* 0,1 */
      - P[1][1],            /* 1,0 */
      Q_gyro                        /* 1,1 */
    };

     const float      PdotX[2 * 2] = {
      Q_angle - P1[0][1] - P1[1][0],      /* 0,0 */ //(Kalman)
      - P1[1][1],                    /* 0,1 */
      - P1[1][1],                    /* 1,0 */
      Q_gyro                        /* 1,1 */
    };
    
    const float      PdotY[2 * 2] = {
      Q_angle - P2[0][1] - P2[1][0],      /* 0,0 */ //(Kalman)
      - P2[1][1],                    /* 0,1 */
      - P2[1][1],                    /* 1,0 */
      Q_gyro                        /* 1,1 */
    };
    
    const float      PdotZ[2 * 2] = {
      Q_angle - P3[0][1] - P3[1][0],      /* 0,0 */ //(Kalman)
      - P3[1][1],                    /* 0,1 */
      - P3[1][1],                    /* 1,0 */
      Q_gyro                        /* 1,1 */
    };

    /* Store our unbias gyro estimate */
    rate = q; //(Kalman)
    rate_X = q_X; //(Kalman)
    rate_Y = q_Y;
    rate_Z = q_Z;

    /*
       * Update our angle estimate
            * angle += angle_dot * dt
            *       += (gyro - gyro_bias) * dt
            *       += q * dt
            */
    angle += q * dt; //(Kalman)
    angleX += q_X * dt; //(Kalman)
    angleY += q_Y * dt;
    angleZ += q_Z * dt;

    /* Update the covariance matrix */
    P[0][0] += Pdot[0] * dt; //(Kalman)
    P[0][1] += Pdot[1] * dt; //(Kalman)
    P[1][0] += Pdot[2] * dt; //(Kalman)
    P[1][1] += Pdot[3] * dt; //(Kalman)

    P1[0][0] += PdotX[0] * dt; //(Kalman) X axis
    P1[0][1] += PdotX[1] * dt; //(Kalman)
    P1[1][0] += PdotX[2] * dt; //(Kalman)
    P1[1][1] += PdotX[3] * dt; //(Kalman)
  
    P2[0][0] += PdotY[0] * dt;//y axis
    P2[0][1] += PdotY[1] * dt;
    P2[1][0] += PdotY[2] * dt;
    P2[1][1] += PdotY[3] * dt;  
    
    P3[0][0] += PdotZ[0] * dt;//y axis
    P3[0][1] += PdotZ[1] * dt;
    P3[1][0] += PdotZ[2] * dt;
    P3[1][1] += PdotZ[3] * dt;  

    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)
    {
      print (); //(NunChuck)
    }
    cnt = 0; //(NunChuck)

    R_angle= (joy_y_axis+1)*0.0098039; //external adjust jitter of accelerometer with nunchuck joystick


    const float            angle_m = atan2( ax_m, az_m ); //(Kalman)
    const float            angle_err = angle_m - angle; //(Kalman)
    const float            C_0 = 1; //(Kalman)
    const float            PCt_0 = C_0 * P[0][0];  //(Kalman)
    const float            PCt_1 = C_0 * P[1][0]; //(Kalman)
    const float            E =R_angle+ C_0 * PCt_0; //(Kalman)
    const float            K_0 = PCt_0 / E; //(Kalman)
    const float            K_1 = PCt_1 / E; //(Kalman)             
    const float            t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] (Kalman) */

    const float            t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1]  = 0 (Kalman) */


    P[0][0] -= K_0 * t_0; //(Kalman)
    P[0][1] -= K_0 * t_1; //(Kalman)
    P[1][0] -= K_1 * t_0; //(Kalman)
    P[1][1] -= K_1 * t_1; //(Kalman)
    angle      += K_0 * angle_err; //(Kalman)
    q_bias      += K_1 * angle_err; //(Kalman)
    
        const float            angle_mX = atan2( ax_m, az_m ); //(Kalman) X axis
    const float            angle_errX = angle_mX - angleX; //(Kalman) X axis
    const float            C_0_1 = 1; //(Kalman) X axis
    const float            PCt_0_1 = C_0_1 * P[0][0];  //(Kalman)
    const float            PCt_1_1 = C_0_1 * P[1][0]; //(Kalman)
    const float            E_1 =R_angle+ C_0_1 * PCt_0_1; //(Kalman)
    const float            K_0_1 = PCt_0_1 / E_1; //(Kalman)
    const float            K_1_1 = PCt_1_1 / E_1; //(Kalman)             
    const float            t_0_1 = PCt_0_1; /* C_0_1 * P[0][0] + C_1 * P[1][0] (Kalman) */
    const float            t_1_1 = C_0_1 * P[0][1]; /* + C_1 * P[1][1]  = 0 (Kalman) */


    P1[0][0] -= K_0_1 * t_0_1; //(Kalman)
    P1[0][1] -= K_0_1 * t_1_1; //(Kalman)
    P1[1][0] -= K_1_1 * t_0_1; //(Kalman)
    P1[1][1] -= K_1_1 * t_1_1; //(Kalman)
    angleX      += K_0_1 * angle_errX; //(Kalman)
    q_bias_X      += K_1_1 * angle_errX; //(Kalman)
    
    
    const float         angle_mY = atan2( ay_m, az_m );  // y axis
    const float         angle_errY = angle_mY - angleY;  // y axis
    const float         C_0_2 = 1;
    const float         PCt_0_2 = C_0_2 * P2[0][0];
    const float         PCt_1_2 = C_0_2 * P2[1][0];
    const float         E_2 =R_angle+ C_0_2 * PCt_0_2;
    const float         K_0_2 = PCt_0_2 / E_2;
    const float         K_1_2 = PCt_1_2 / E_2;
    const float         t_0_2 = PCt_0_2;
    const float         t_1_2 = C_0_2 * P2[0][1];
    
    P2[0][0] -= K_0_2 * t_0_2;
    P2[0][1] -= K_0_2 * t_1_2;
    P2[1][0] -= K_1_2 * t_0_2;
    P2[1][1] -= K_1_2 * t_1_2;
    angleY += K_0_2 * angle_errY;
    q_bias_Y += K_1_2 * angle_errY;
    
    const float         angle_mZ = averageZ/5;  
    const float         angle_errZ = angle_mZ - angleZ;  
    const float         C_0_3 = 1;
    const float         PCt_0_3 = C_0_3 * P3[0][0];
    const float         PCt_1_3 = C_0_3 * P3[1][0];
    const float         E_3 =R_angle+ C_0_3 * PCt_0_3;
    const float         K_0_3 = PCt_0_3 / E_3;
    const float         K_1_3 = PCt_1_3 / E_3;
    const float         t_0_3 = PCt_0_3;
    const float         t_1_3 = C_0_3 * P3[0][1];
    
    P3[0][0] -= K_0_3 * t_0_3;
    P3[0][1] -= K_0_3 * t_1_3;
    P3[1][0] -= K_1_3 * t_0_3;
    P3[1][1] -= K_1_3 * t_1_3;
    angleZ += K_0_3 * angle_errZ;
    q_bias_Z += K_1_3 * angle_errZ;

    rotationZ = angleZ;
 
    Serial.print("average z ");Serial.print(averageZ);Serial.print("  lastaverage z ");Serial.println(lastaverageZ);
    Serial.print("  angle_mZ = "); Serial.println(int(angle_mZ * 57.2957795130823));
    Serial.print("Rate  ");Serial.print(int(rate*57.2957795130823)); Serial.print("  Angle X  "); Serial.print(int(angleX*57.2957795130823)); Serial.print("  Angle Y  "); Serial.println(int(angleY*57.2957795130823)); //Prints degrees Acceleromter+Gyros+KalmanFilters
    Serial.print(joy_y_axis); //Prints the adjust for accelerometer jitter
    Serial.print(" ");
    Serial.print(int(angle*57.2957795130823)); //Prints degrees Acceleromter+Gyros+KalmanFilters
    Serial.print(" ");
    Serial.print(int(angle_m*57.295779513082)); //Prints the accelometer
    Serial.print(" ");
    Serial.println(joy_x_axis); //Prints the Gyro adjusment
  }
}

void print ()//This is the function to get the bytes from nintendo wii nunchuck
{
  joy_x_axis = outbuf[0];
  joy_y_axis = outbuf[1];
  ax_m = (outbuf[2] * 2 * 2) -511; //Axis X Acceleromter
  ay_m = (outbuf[3] * 2 * 2) -511; //Axis Y Acceleromter
  az_m = (outbuf[4] * 2 * 2) -511; //Axis Z Acceleromter

  // 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;
  }


}
« Last Edit: July 29, 2009, 04:04:29 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

I have been doing the same thing. Problem is the Kalman filter.
                   Kalman Filter
6DOF >>>>>> MAGIC >>>>>> Data out.

The MP+ and NC will make a good 6DOF. I have been following on the posts. Used Knuckles 2 transistor method for right now. The combo must be able to be read simultaneously because the new Wii games do. I'll look at the data while game hooked up.
Earl


Logged

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

Great stuff, I was using jordi's code as well and thats more or less what Ive put together. How has tuning it gone so far? From what Ive read, thats usually the hard part. I havent had a chance to test much due to moving apartments right now. Probably wont get a chance for at least a week now. Its great to see that Ive got some great partners in crime tho.
Logged

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

@uphiearl: Its working fine doing the samples now, and seems to be smoothing very well, certainly on the "Roll" Axis.. Kalman isn't entirely magic.. it has taken me a bit to get my head around it.. but once you start manipulating the code you'll see its not *that* complex or mystical !! smiley

@Knuckles: Tell me about it. Biggest hassle is 'decoding' the mis-named axis's of the WM+ (yes, I know you ported those names across for legacy reasons !!  smiley-wink ).  I need to re-do the breakout board I made so I can move the thing more than 6inches from the arduino smiley-wink At least using Jordi's code base we can 'tweak' the calibrations using the Nunchuck joystick - and then convert those off-sets back into the code... reduces the whole "code it, load it, run it" cycle a *heap* !

Just a general note::

I know most people will already be ontop of it, but make sure that the Nunchuck and WM+ are joined together such that they form a Rigid Member! Also make sure that you reset the arduino to get correct zero's and then it should produce a wonderfully smoother roll-angle. If the nunchuck and WM+ are NOT rigid, then you will get inexplicable drifts.

The pitch and yaw's are still 'works in progress' since there is no tuning yet, and looking at it today, I think I have to swap the axis' about (make yaw pitch and pitch yaw)...
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

Great...can you post the new code ?
Earl
Logged

London
Offline Offline
Tesla Member
***
Karma: 10
Posts: 6250
Have fun!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
Are the times here GMT ?
this question is probably better placed in the FAQ section but you can set the offset from GMT as an option. I don't think there is a user friendly way of getting to your options settings, I use the link with the user name appended:
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?action=myprofileOptions;username=
(put your  user name after the equals sign)
« Last Edit: July 31, 2009, 02:10:06 am by mem » Logged

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

Hi Adr1an -

Thanks for the post.

I've looked through your code and have a few comments.

1. In Jordi's orginal 'loop' method code, he would subtract the previous reading from the total, read the new value and add it to the total, then take the average.  In your 'loop', you are only adding it - and it's being added before reading the current values, which is incorrect.  Shouldn't it be more along these lines?:
Code:
void loop()
{
  if((millis()-lastread) >= mydt) { // sample every dt ms -> 1000/dt hz.
    lastread = millis();
    totalX -= readingsX[index];
    totalY -= readingsY[index];
    totalZ -= readingsZ[index];               // subtract the reading from the total
    switchtowmp();
    receiveData();
    switchtonunchuck();

    readingsX[index] = int(roll/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
    readingsY[index] = int(pitch/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
    readingsZ[index] = int(yaw/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
    totalX += readingsX[index];
    totalY += readingsY[index];
    totalZ += readingsZ[index];

    index = (index + 1);                    // advance to the next index
...
...

2. Also in the 'loop' method, there are computations for q_m_X, q_m_Y, and q_m_Z.  All 3 of those equations use joy_x_axis.  Is that intended?  Shouldn't q_m_Y use joy_y_axis?  At a more basic level, I don't understand why Jordi had the need to use the joystick values here anyway.  It seems odd.  If you were to just use a non-Nunchuck 3-axis accelerometer, you would not have those values.  I need to study his math more to fully understand I guess.
3. I think there are cut-and-paste errors in the PCt_0_1, PCt_1_1, and t_1_1 constant equations (Should be P1 instead of P).
Code:
   const float            PCt_0_1 = C_0_1 * P1[0][0];  //(Kalman)
    const float            PCt_1_1 = C_0_1 * P1[1][0]; //(Kalman)
    const float            t_1_1 = C_0_1 * P1[0][1]; /* + C_1 * P[1][1]  = 0 (Kalman) */


I also think that there's a lot of unnecessary logic in the code that can be removed.  For example readings, total, average, and P are dupes of readingsX, totalX, averageX, and P1.  I don't think that affects the outcome, just memory size and processing time.

Even with these changes my code is acting wacky.  I'll keep investigating.

Cheers -
Duck
Logged

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

@Duck: Yes! my original paste here was when I had accidentally deleted the second set of -'s - so the correction is correct.

As for the joy_x part, its just a nasty hack to calibrate all gyros off one axis.

I suspect the calibration needs more tweaking - i'm halfway through playing with it and have a clean y-axis and an almost clean x-axis... Z is just off-the-planet at the moment smiley-wink

re logic that can be culled: I have no doubt - its really a massage of about 4 sets of code, so lots of it is probably now unrequired or in just an ugly state in general. the original set of variables without the X on them are just for a quick way to jump back to jordi's original code.. they are entirely artifacts and can be ditched...

Try tweaking the 0.3 calibration part for each axis.
Also - I know my degrees calculation needs some extra work - it needs to pickup the LSB from the WM+ to detect fast or slow rotation.. the resolution does appear to be different...
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

@Duck
Any more progress ?
Logged

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

Slowly but surely.  I hope to make a lot more progress this evening.  Here's one small fix that I posted on another blog.  (Note that I have switched roll and pitch from Adrian's (and other's) posts to make it inline with http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus data structure definition.  
Code:
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;
}

Some other changes that I'm making is honoring the dual speed modes of the Wii Motion Plus.  There are 3 extra bits in the data that indicate if the output data is fast or slow.  This presumably maps directly to the 500 degree/sec and the 2000 degree/sec modes of the Invensense IDG-600.

I also went all the way back to the original articles and code by Tom Pycke.  A lot of changes have been made along the way - not all of them valid.  Plus I wanted to wrap my head around the real issues with Kalman Filtering.

I'll post more soon if I can make progress.

Duck
Logged

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

If I can be of any help let me know. I have both a 368 and a 168 arduino and a MP+ and NC wired together and a spare NC and spare WM+.
I am starting to get the hang of the arduino programming. I am using 016...
Earl
Logged

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

On the 500/sec or 2000/sec on the idg 600, My impression is that it is a factory set calibration that you cant change,. Or is it not like the idg 650 ?
Earl
Logged

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

Earl: It signifies what mode its in by 1's or 0's in the last 3 bytes of the payload data... you then have to alter the degrees/sec calculations based on if its in 'fast' mode or 'slow' mode...

Duck: Thanks heaps... I tried to keep the history chain of code valid... Looks like your making great progress.
I Know the Pitch/Roll/Yaw is backwards - but that was because I was deliberately maintaining the naming from Knuckles code... hence the need to use 'pitch' for the 'roll' correction etc etc.

I've been stuck making a new board for the last few days... I should be able to finish my other calibration tests soon.

Keep it coming Duck !! smiley
(Kalman... its interesting isn't it smiley-wink )
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

Oh., I thought the 6 bytes of data were xyzXYZ. You combine the xX yY and zZ to get the 10 bit resolution of the a/d converter. Or for one resolution you only use 8 bits and for the higher resolution you use the all 10 bits?
« Last Edit: August 04, 2009, 08:32:45 pm by uphiearl » Logged

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