|
|
本帖最後由 tommylin 於 2012-9-9 12:51 編輯
PART 10: Wii motion plus + Nunchuck
我知道這已經過時很久了,
但是我蹺課太久了,現在捕回來@@
找到下列程式有一個重點,就是經過了 "Kalman Filter"的計算,
這樣才能夠實際應用, 不然一堆雜訊是沒辦法用的.
但是如果要更好的效果, 就要選用 9DOF IMU,
搭配Compass一起計算...這是後面的課題了..- // 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.write (0xF0); // sends memory address
- Wire.write (0x55); // sends sent a zero.
- Wire.endTransmission (); // stop transmitting
- delay(100);
- Wire.beginTransmission (0x52); // transmit to device 0x52
- Wire.write (0xFB); // sends memory address
- Wire.write (0x00); // sends sent a zero.
- Wire.endTransmission (); // stop transmitting
- }
|
|
發表於 2011-2-25 23:52:36
樓主
發表於 2012-9-10 11:57:40
