My Ardu-IMU, Use a sparkfun Gyro, Wii accelerometers and Kalman Filters (code). Is working now, but still beta...
You need to be a member of diydrones to add comments!
My Ardu-IMU, Use a sparkfun Gyro, Wii accelerometers and Kalman Filters (code). Is working now, but still beta...
You need to be a member of diydrones to add comments!
Comments
I am very interest in you project, and I also want to make my own IMU, but i dont know how to process the data using Kalman Filters, can you send me some code about the Kalman Filters in the IMU, or some resource about that. Thanks a lot.
try again using the nice little file upload thing.
Thanks Chris..
wii_nunchuck2.pde
we are still waiting for the code... :) i want to experiment with wii nunchuck accelerometer but have not broken down the nunchuck yet... :) i managed to read nunchuck data (z-data is not satisfactory) with my DIY connector but could not find a good kalman filter software to process the data... :(
void updateServos()
{
if((millis() - lastPulse) >= refreshTime) {
digitalWrite(servoPin_x, HIGH);
delayMicroseconds(pulseWidth_x);
digitalWrite(servoPin_x, LOW);
digitalWrite(servoPin_y, HIGH);
delayMicroseconds(pulseWidth_y);
digitalWrite(servoPin_y, LOW);
lastPulse=millis();
}
}
int index=0; // inrement this each print cycle
void printNunchuckData()
{
joy_x_axis = outbuf[0];
joy_y_axis = outbuf[1];
accel_x_axis = outbuf[2];
accel_y_axis = outbuf[3];
accel_z_axis = outbuf[4];
z_button = 0;
c_button = 0;
// you need to add in the two lsb's from outbuf[5]
if((outbuf[5] >> 0) & 1)
z_button = 1;
if((outbuf[5] >> 1) & 1)
c_button = 1;
//
if((outbuf[5] >> 2) & 1)
accel_x_axis += 2;
if((outbuf[5] >> 3) & 1)
accel_x_axis += 1;
if((outbuf[5] >> 4) & 1)
accel_y_axis += 2;
if((outbuf[5] >> 5) & 1)
accel_y_axis += 1;
if((outbuf[5] >> 6) & 1)
accel_z_axis += 2;
if((outbuf[5] >> 7) & 1)
accel_z_axis += 1;
Serial.print(index, DEC);
Serial.print("\t");
Serial.print(joy_x_axis, DEC);
Serial.print("\t");
Serial.print(joy_y_axis, DEC);
Serial.print("\t");
Serial.print(accel_x_axis, DEC);
Serial.print("\t");
Serial.print(accel_y_axis, DEC);
Serial.print("\t");
Serial.print(accel_z_axis, DEC);
Serial.print("\t");
Serial.print(z_button, DEC);
Serial.print(" ");
Serial.print(c_button, DEC);
Serial.print("\r\n");
index++;
}
// decode nunchuck data
char nunchuck_decode_byte (char x)
{
x = (x ^ 0x17) + 0x17;
return x;
}
/*
* wii nunchuck servo system
* Phil Wilshire
* July 4th, 2008
* Embedded Linux Developer
* http://www.sysdcs.com
* This based totally on the work by Tod E. Kurt,
* http://todbot.com/blog
* who based it on the Winmeadow Labs work
* http://www.winmeadow.com/node/42
*
* moved it around a bit and added an extra axis
*
* This is my first venture into Arduino code.
* this was running on a Boarduino
* It looks like a load of fun
*/
#include
uint8_t outbuf[6];
int cnt = 0;
int ledPin = 13;
int servoPin_x = 7;
int servoPin_y = 6;
int pulseWidth_y = 0;
int pulseWidth_x = 0;
long lastPulse = 0;
int refreshTime = 20;
int minPulse = 700;
int maxPulse = 1700;
int joy_x_axis;
int joy_y_axis;
int accel_x_axis;
int accel_y_axis;
int accel_z_axis;
int z_button = 0;
int c_button = 0;
#define pwbuffsize 4
#define pwbuffnum 3 // 0 = x 1 = y 2 = z
int pwbuff[pwbuffnum][pwbuffsize];
int pwbuffpos = 0;
int pulsewidth[pwbuffnum];
int curmode = 0;
/*
* Boarduino connections
* If you get smoke you may not have checked these properly
*
* connect x servo to dig 7 last on the top left
* (with the power connector to the left)
* connect y servo to dig 6 one pin to the right of dig 7
* connect +5 to +5v 9 pins in from left
* connect 0v to gnd 10 pins in from left
* nunchuck clock (yellow) to an 5, last one on the bottom right
* nunchuck data (green) to an 4, one pin to the left of an 5
*/
void setup()
{
beginSerial(9600); // 19200 did not work for me
Wire.begin();
nunchuck_init(); // set up I2C
pinMode(servoPin_x, OUTPUT); // A couple of servo outputs
pinMode(servoPin_y, OUTPUT);
for(int p = 0 ; p < pwbuffnum; p++)
pulsewidth[p] = minPulse;
Serial.print("Finished setup\n");
curmode = 0; // set stick control
}
void nunchuck_init()
{
Wire.beginTransmission(0X52); // start sending to address 0x52
Wire.send(0x40); // send 0x40
Wire.send(0x00); // send 00
Wire.endTransmission(); // stop sending
}
void send_zero()
{
Wire.beginTransmission(0X52); // start sending to address 0x52
Wire.send(0x00); // send 00
Wire.endTransmission(); // stop sending
}
int t = 0; // read nunchuck when it gets to 25
void read_nunchuck()
{
Wire.requestFrom(0x52, 6);
while(Wire.available()) {
outbuf[cnt] = nunchuck_decode_byte(Wire.receive());
digitalWrite(ledPin, HIGH);
cnt++;
}
}
void processData()
{
float axis[3];
for(int p = 0 ; p < pwbuffnum; p++) {
axis[p] = outbuf[p+3];
axis[p] = (axis[p]-70) * 1.5;
pulsewidth[p] = (axis[p] * 9) + minPulse;
pwbuff[p][pwbuffpos] = pulsewidth[p];
pulsewidth[p] = 0;
}
if(++pwbuffpos == pwbuffsize) pwbuffpos=0;
for (int j = 0; j
for (int p = 0; p
pulsewidth[p] += pwbuff[p][j];
}
}
for (int p = 0; p
pulsewidth[p] /= pwbuffsize;
}
}
void loop()
{
t++;
if(t == 25) {
t = 0;
read_nunchuck();
if(cnt >= 5) {
printNunchuckData();
processData();
if (curmode == 0) { //press Z for acelleration control
pulseWidth_x=pulsewidth[0];
pulseWidth_y=pulsewidth[1];
}
}
if (curmode == 1) { // press c for direct control
pulseWidth_y=minPulse + (joy_y_axis * 4);
pulseWidth_x=minPulse + (joy_x_axis * 4);
}
if (z_button == 0) { //press Z for acelleration control
curmode = 0;
}
if (c_button == 0) { // press c for direct control
curmode = 1;
}
cnt = 0;
send_zero();
}
updateServos();
delay(1);
}
void updateServos()
{
if((millis() - lastPulse) >= refreshTime) {
digitalWrite(servoPin_x, HIGH);
delayMicroseconds(pulseWidth_x);
digitalWrite(servoPin_x, LOW);
I got the nunchuck working during my July 4th break from my other projects.
Any idea what the nunchuck chip is and if we can get it ??
I'll post the code in the next comment.
regards
Phil WIlshire