但是我蹺課太久了,現在捕回來@@
這樣才能夠實際應用, 不然一堆雜訊是沒辦法用的.
但是如果要更好的效果, 就要選用 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
}
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.write (0x00); // sends one byte (nunchuck)
Wire.endTransmission (); // stop transmitting (nunchuck)
}
void wmpOn(){
Wire.beginTransmission(0x53);//WM+ starts out deactivated at address 0x53
Wire.write(0xfe); //send 0x04 to address 0xFE to activate WM+
Wire.write(0x04);
Wire.endTransmission(); //WM+ jumps to address 0x52 and is now active
}
void wmpOff(){
Wire.beginTransmission(82);
Wire.write(0xf0);//address then
Wire.write(0x55);//command
Wire.endTransmission();
}
void wmpSendZero(){
Wire.beginTransmission(0x52);//now at address 0x52
Wire.write(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.read();
}
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.read();
}
//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.read();
}
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
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.read (); // 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;
}
}
沒有留言:
張貼留言