Download - Modified Kalman Code Using Roll
// 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 Adrian Carter, Knuckles904, evilBunny, Ed Simmons, & Jordi Munoz//// There are two coordinate systems at play here:// The body-fixed coordinate system, which is fixed to the platform. That is, the x-axis is always pointing to the nose of the// aircraft (nosecone on a rocket), the y-axis points to the right, and the z-axis points down on an airplane, (parallel to the// earth away from you on a rocket).//// The aircraft will move with respect to the navigation frame, which is a fixed coordinate system. // In that frame, the x-axis points north, the y-axis points east and the z-axis points down. This is called the "NED system", or// the "Local Tangent Plane" or the "Local Geodetic Frame".
// Created by Duckhead v0.6 Yaw is done via integration of gyro data and smoothed via a Runge-Kutta 4, but there are still issues with it
#include <math.h>#include <Wire.h>/////////////////////////////////////////
struct GyroKalman{ /* These variables represent our state matrix x */ double x_angle, x_bias;
/* Our error covariance matrix */ double 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 */ double Q_angle, Q_gyro;
/* * Covariance R, our observation noise (from the accelerometer)
* Also assumed to be linear with dt */ double R_angle;};
struct RungeKutta { double val_i_3; double val_i_2; double val_i_1; double previous;};
//Precomputed constants to avoid floating point division at runtime (too many clock cycles)static const double RadianToDegree = 57.2957795; // 180/PIstatic const double DegreeToRadian = 0.0174532925; // PI/180
//The WMP has two modes: 2000 deg/s (slow) and 500 deg/s (fast). It is assumed that the sensitivity of the output follows//the same ratio (it seems that the sensitivity is 20 mV/deg/s and 0.5 mV/deg/s), which is to say it's proportional to 4:1.static const int wmpSlowToDegreePerSec = 20;static const int wmpFastToDegreePerSec = wmpSlowToDegreePerSec/5;
//The system clock function millis() returns a value in milliseconds. We need it in seconds.//Instead of dividing by 1000 we multiply by 0.001 for performance reasons.static const double SecondsPerMillis = 0.001;
//WM+ variables/* * 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. * * You should play with different values here as the effects are interesting. Over prioritizing the * accelerometers results in fairly inaccurate results. */static const double Q_angle = 0.002; //(Kalman)static const double Q_gyro = 0.1; //(Kalman)
// Kalman data structures for each rotational axisstruct GyroKalman rollData;struct GyroKalman pitchData;struct GyroKalman yawData;struct RungeKutta yawRK;
// raw 3 axis gyro readingsint readingsX;int readingsY;int readingsZ;
byte data[6]; //six data bytes
// normalized (scaled per WM+ mode) rotation valuesint yaw = 0;
int pitch = 0;int roll = 0;
// calibration zeroesint yaw0 = 0;int pitch0 = 0;int roll0 = 0;
// WM+ state variables - if true, then in slow (high res) modebool slowYaw;bool slowPitch;bool slowRoll;bool debug = false;
// Nunchuck variables
float accelAngleX=0; //NunChuck X anglefloat accelAngleY=0; //NunChuck Y anglefloat accelAngleZ=0; //NunChuck Z anglefloat theta=0; //Angle of the z-axis to vertical (not the same as yaw)
// Internal rogram state variablesunsigned long lastread=0; //last system clock in millis
/* * Nunchuk accelerometer value to radian conversion. Normalizes the measured value * into the range specified by hi-lo. Most of the time lo should be 0, and hi should be 1023. * * lo the lowest possible reading from the Nunchuk * hi the highest possible reading from the Nunchuk * measured the actual measurement */float angleInRadians(int lo, int hi, int measured) { float x = (hi - lo); return (float)(measured/x) * PI;}
/* * Switch control from WM+ to Nunchuck via I2C. */void switchtonunchuck(){ digitalWrite(3, LOW); digitalWrite(4, LOW); digitalWrite(4, HIGH);}
/* * Switch control from Nunchuck to WM+ via I2C. */void switchtowmp(){ digitalWrite(3, LOW); digitalWrite(4, LOW); digitalWrite(3, HIGH);}
/* * Initialize Nunchuck. */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}
/* * Initialize everything. */void setup(){ Serial.begin(115200);
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); yawRK.val_i_1 = 0; yawRK.val_i_2 = 0; yawRK.val_i_3 = 0; yawRK.previous = 0; lastread = millis();}
/* * Initialize the kalman structures. * * kalman the kalman data structure * Q_angle the process covariance noise for the accelerometers * Q_gyro the process covariance noise for the gyros * R_angle the measurement covariance noise (jitter in the accelerometers) */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 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();}
/* * 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(){ long y0 = 0; long p0 = 0; long r0 = 0; const int avg = 100; 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;}
//STITCH
//STITCH
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]; 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;
//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 loop(){ unsigned long now = millis(); double dt = ((double)(now - lastread)) * SecondsPerMillis; //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 predict(&rollData, readingsX * DegreeToRadian, dt); predict(&pitchData, readingsY * DegreeToRadian, dt); double rollAngle = update(&rollData, accelAngleX); double pitchAngle = update(&pitchData, accelAngleY); double yawAngle = computeRK4(&yawRK, readingsZ*DegreeToRadian*dt);
// Serial.print(" Roll: "); Serial.print(rollAngle*RadianToDegree); Serial.print(", ");// Serial.print(" Pitch: "); Serial.print(pitchAngle*RadianToDegree); Serial.print(", ");// Serial.print(" Yaw: "); Serial.print(yawAngle*RadianToDegree); Serial.print(", "); Serial.print(1); Serial.print(","); Serial.println(2);// Serial.print(" Theta: ");// Serial.println(theta*RadianToDegree); }}
/* * 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, double dotAngle, double 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 */double update(struct GyroKalman *kalman, double angle_m) { const double y = angle_m - kalman->x_angle; const double S = kalman->P_00 + kalman->R_angle; const double K_0 = kalman->P_00 / S; const double 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;}
/* * Compute the common fourth-order Runge-Kutta algorithm as part of the integrating the gyro's yaw signal. This * will smooth the values a tad. Gyro drift is still a problem, however. * * rk the RungaKutta struct to hold previous values * val_i_0 the latest raw value to integrate * * returns the RK4 approximation of all values up until time t */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;}
/* * Write a zero byte to the nunchuck. */void send_zero () //(nunchuck){ Wire.beginTransmission (0x52); // transmit to device 0x52 (nunchuck) Wire.send (0x00); // sends one byte (nunchuck) Wire.endTransmission (); // stop transmitting (nunchuck)}
/* * Request and read the current accelerometer values from the Nunchuck. */void readNunchuck () { int cnt = 0; byte outbuf[6]; // array to store NunChuck output
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 received the 6 bytes, then process them //(NunChuck) if (cnt >= 5) //(NunChuck) {
processAccelerometers (outbuf); //(NunChuck) }}
/* * This function processes the byte array from the wii nunchuck. * * The execution of this function results in the updating of 3 globals, accelAngleX, accelAngleY, and accelAngleZ with the angle in radians. * If the Wii Motion Plus and the Nunchuck are oriented together in the way this code assumes then: * Nunchuck X maps to Wii Motion Plus Roll * Nunchuck Y maps to Wii Motion Plus Pitch * Nunchuck Z maps to Wii Motion Plus Yaw * */void processAccelerometers (byte outbuf[]){ //Pull the bits out of the array for each axis. Other implementations floating around are incorrect with the way //they operate on the lower 2 bits. Thanks to evilBunny for this code. int ax_m = (outbuf[2] << 2) + ((outbuf[5] >> 2) & 0x03); int ay_m = (outbuf[3] << 2) + ((outbuf[5] >> 4) & 0x03); int az_m = (outbuf[4] << 2) + ((outbuf[5] >> 6) & 0x03);
//The nunchuk accelerometers read ~511 steady state. Remove that to zero the values. ax_m -= 511; ay_m -= 511;
az_m -= 511;
if (debug) { Serial.print("ax: "); Serial.print(ax_m); Serial.print("ay: "); Serial.print(ay_m); Serial.print("az: "); Serial.println(az_m); }
//The real purpose of this method is to convert the accelometer values into usable angle values. //Knuckles904 pointed me to this app note: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf //That paper gives the math for implementing a tilt sensor using 3-axis accelerometers. Roll(X) and pitch(Y) are directly //applicable. Yaw(Z) is not since there is no 'tilt' with respect to the earth. //Once the accelerometer values have been biased to zero (by subtracting 511 above), then they should fall in a range from //-512 to +511. double x = angleInRadians(-512, 511, ax_m); double y = angleInRadians(-512, 511, ay_m); double z = angleInRadians(-512, 511, az_m);
//compute values that are used in multiple places double xSquared = x*x; double ySquared = y*y; double zSquared = z*z;
accelAngleX = atan(x / sqrt(ySquared + zSquared)); accelAngleY = atan(y / sqrt(xSquared + zSquared)); theta = atan(sqrt(xSquared + ySquared) / z); }
Back to top
#include <AXELCD.h>
#include <MD23.h>#include <Wire.h>
#include <math.h>
/////////////////////////////////////////#define NUMREADINGS 5 //Gyro noise filterint readings[NUMREADINGS]; // the readings from the analog input (gyro)int index = 0; // the index of the current readingint total = 0; // the running totalint average = 0; // the average
int inputPin =0; //Gyro Analog input///////////////////////////////////////
float dt = .1; // .06; //( 1024.0 * 256.0 ) / 16000000.0; (Kalman) // what does this means ?? hand tunedint mydt = 20; //in ms.static float PP[2][2] = { //(Kalman) { 1, 0 } , //(Kalman) { 0, 1 } ,//(Kalman)}; //(Kalman)
/* * Our two states, the angle and the gyro bias. As a byproduct of computing * the angle, we also havef an unbiased angular rate available. These are * read-only to the user of the module. */float angle = 0.0; //(Kalman)float q_bias = 0.0; //(Kalman)float rate = 0.0; //(Kalman)float q_m = 0.0; //(Kalman)
int ax_m=0;int ay_m=0;int cnt = 0; //Counterunsigned long lastread=0;unsigned long startmillis=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)int ledPin = 13;
MD23 md23;AXELCD lcd;
byte displayState = 0; // 0 clean,float oldAngle = 0.0; //
float P = 0.0;float I = 0.0;float D = 0.0;
void setup(){ pinMode(ledPin, OUTPUT); pinMode(2, INPUT); // declare pushbutton as input
delay(500); Wire.begin(); // init i2c md23.setMode(0); // set the mode (0 is default, so not really useful :P) md23.disableSpeedRegulation(); lcd.clearDisplay(); // this always need 30 millis delay. delay(30); lcd.write("Chiappe!"); delay(10); lcd.moveLine2(0); delay(10); lcd.write("Battery:"); // every other command needs 10 millis delay delay(10);
byte b = md23.getBatteryVolts(); lcd.write(b, DEC); delay(10); lcd.moveLine2(0); delay(10);
Serial.begin(9600); for (int i = 0; i < NUMREADINGS; i++) readings[i] = 0; // initialize all the readings to 0 (gyro average filter)
startmillis = millis();
}float sqr(float a){ return a*a; }float sgn (float a) { if (a > 0) return +1; else if (a < 0) return -1; else return 0;}
long readMAX127(byte chan){ byte control = 0x80 + (chan << 4); byte addr = 0x28;
Wire.beginTransmission(addr); Wire.send(control); // send value Wire.endTransmission();
Wire.requestFrom(addr, (byte)2);
byte b1 = Wire.receive(); byte b2 = Wire.receive() >> 4; long data = (b1 << 4) | (b2 & 0x0F);
return data;}
void loop(){ int delta = millis()-lastread; if( delta >= mydt) { // sample every dt ms -> 1000/dt hz. lastread = millis(); total -= readings[index]; // subtract the last gyro reading readings[index] = analogRead(2); // read from the gyro sensor total += readings[index]; // add the reading to the total 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)-500; // calculate the average of gyro input
dt = ((float)delta) / 1000.0;
q_m= ((float)average)*(1500.0/1024.0)*PI/180 ; // HAC remove 1.5 mult
/* Unbias our gyro */ const float q = q_m - q_bias; //(Kalman)
const float Pdot[2 * 2] = { Q_angle - PP[0][1] - PP[1][0], /* 0,0 */ //(Kalman) - PP[1][1], /* 0,1 */ - PP[1][1], /* 1,0 */ Q_gyro /* 1,1 */ };
/* Store our unbias gyro estimate */ rate = q; //(Kalman)
/** Update our angle estimate * angle += angle_dot * dt * += (gyro - gyro_bias) * dt * += q * dt */ angle += q * dt; //(Kalman)
/* Update the covariance matrix */ PP[0][0] += Pdot[0] * dt; //(Kalman) PP[0][1] += Pdot[1] * dt; //(Kalman) PP[1][0] += Pdot[2] * dt; //(Kalman) PP[1][1] += Pdot[3] * dt; //(Kalman)
// read here! ax_m = analogRead(0)-510; ay_m = analogRead(1)-510;
// non aggiusto R // R_angle= (joy_y_axis+1)*0.0098039; //external adjust jitter of accelerometer with nunchuck joystick
const float angle_m = atan2( ay_m, ax_m ); //(Kalman) const float angle_err = angle_m - angle; //(Kalman) const float C_0 = 1; //(Kalman) const float PCt_0 = C_0 * PP[0][0]; //(Kalman) const float PCt_1 = C_0 * PP[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 * PP[0][1]; /* + C_1 * P[1][1] = 0 (Kalman) */
PP[0][0] -= K_0 * t_0; //(Kalman) PP[0][1] -= K_0 * t_1; //(Kalman) PP[1][0] -= K_1 * t_0; //(Kalman) PP[1][1] -= K_1 * t_1; //(Kalman) angle += K_0 * angle_err; //(Kalman) q_bias += K_1 * angle_err; //(Kalman)
float calibration = -4.5; // read from a pot or something float myangle=(angle*57.2957795130823)-90.0 + calibration;
if ((millis()-startmillis) > 6000 ) { digitalWrite(ledPin, HIGH);
// float integralAverage = (integral / NUMINTEGRALS);// integral = integral - integralAverage;
int val = digitalRead(2); // read input value if (val == HIGH) { I = 0.0; }
float rate_deg = rate * 57.2957795130823; float pot1 = ((float)readMAX127(1)) ; float pot2 = ((float)readMAX127(2)) ; float pot3 = ((float)readMAX127(3)) ; // myangle is up at 0 // motors is stopped at 128 P = (myangle * (pot1 / 100.0)); // pot/10 D = (myangle-oldAngle) * (pot2 / 10.0); // Kd = 50 (800 at pot)
I = I + myangle * (pot3/100.0); // I * 1.0 + myangle * pot; // 123 looks good float pid = P+I+D; float motors = 128.0 - (pid+(sgn(pid)*sqr(pid/18))); // cap the value if (motors>250.0) motors = 250.0; if (motors<5.0) motors = 5.0; byte motorsInput = (byte)motors; //Serial.println(myangle, DEC); Serial.println(motorsInput, DEC); if ((myangle > 80) || (myangle < -80)) { md23.setMotor1Speed(128); md23.setMotor2Speed(128); I=0; // avoid accumulation of integration when stopped } else { md23.setMotor1Speed(motorsInput); md23.setMotor2Speed(motorsInput); }
oldAngle = myangle;
// update the display displayState ++; if (displayState==7) displayState = 0; switch (displayState) { case 0: lcd.moveLine2(0); break; case 1: lcd.write(pot1, DEC); break; case 2: lcd.write(" "); break; case 3: lcd.write(pot2, DEC); break; case 4: lcd.write(" "); break; case 5:
lcd.write(pot3, DEC); break; case 6: lcd.write(" "); break; } } else { digitalWrite(ledPin, LOW); }
// Serial.print(" "); // Serial.println(int(angle_m*57.295779513082)+180, DEC); //Prints the accelometer// Serial.println(ay_m, DEC); //Prints the accelometer
// Serial.print(" "); //Prints degrees Acceleromter+Gyros+KalmanFilters// Serial.print(" "); }
}
This is the update
/ 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 Knuckles904, Ed Simmons, Jordi Munoz, and Adrian Carter//
// Created by Duckhead v0.3 Nunchuk accelerometers to angles is still not quite right
#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 axisint readingsX;int readingsY;int readingsZ; // the readings// End of 3 axis stuffint index = 0; // the index of the current readingint inputPin =0; // Gyro Analog input//WM+ stuffbyte data[6]; //six data bytesint yaw = 0;int pitch = 0;int roll = 0; //three axesint yaw0 = 0;int pitch0 = 0;int roll0 = 0; //calibration zeroesbool slowYaw;bool slowPitch;
bool slowRoll;
//Precomputed constants to avoid floating point division at runtime (too many clock cycles)static const double RadianToDegree = 57.2957795; // 180/PIstatic const double DegreeToRadian = 0.0174532925; // PI/180
//The WMP has two modes: 2000 deg/s (slow) and 500 deg/s (fast). It is assumed that the sensitivity of the output follows//the same ratio (it seems that the sensitivity is 20 mV/deg/s and 0.5 mV/deg/s), which is to say it's proportional to 4:1.static const int wmpSlowToDegreePerSec = 20;static const int wmpFastToDegreePerSec = wmpSlowToDegreePerSec/5;
///////////////////////////////////////
float accelAngleX=0; //NunChuck X anglefloat accelAngleY=0; //NunChuck Y anglefloat accelAngleZ=0; //NunChuck Z anglebyte outbuf[6]; // array to store arduino outputint cnt = 0; //Counterunsigned 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.4; //(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
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 = 100; 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)) * 0.001; //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 * DegreeToRadian; float tmpY = accelAngleY * DegreeToRadian; float tmpZ = accelAngleZ * DegreeToRadian;
predict(&rollData, readingsX * DegreeToRadian, dt); predict(&pitchData, readingsY * DegreeToRadian, dt); predict(&yawData, readingsZ * DegreeToRadian, dt);
float rollAngle = update(&rollData, tmpX); float pitchAngle = update(&pitchData, tmpY); float yawAngle = update(&yawData, tmpZ);
Serial.print("Roll: "); Serial.print(rollAngle*RadianToDegree); Serial.print(" Pitch: "); Serial.print(pitchAngle*RadianToDegree); Serial.print(" Yaw: "); Serial.println(yawAngle*RadianToDegree); }}//Stitch here