I'm working toward a quadcopter project but as a first step I'm looking to build a two-wheeled balancing-style robot.
A lot of the work with interfacing the combination of the Motion Plus and Nunchuk sensors has been done on the forums here: http://arduino.cc/forum/index.php/topic,8544.0.html, but I'm looking to redo the parseData() function in order to get the data output in something that is more meaningful.
The eventual goal is to turn the raw accelerometer values into G readings, and the gyroscope into deg/sec rate readings, to eventually use them in a complementary filter.
Here are some pictures of the setup I'm using: an original Wii MP extension board and a knockoff nunchuck board. Both are mounted and soldered together, powered by a 3.3v regulator.
Images hosted on Box.com but I can't embed them directly, so here are the links:
https://www.box.com/s/lpao736l5yh7md0ab9tk
https://www.box.com/s/q84y91g8me4fp549d6wd
https://www.box.com/s/g37pihrz38v4pldgef8t
The last image is the breadboard held at a 45-degree angle for testing the calibration.
Right now the problem I'm encountering is the fact that the values output do not correlate linearly to the acceleration they should be reading. My code generates a +/- 1000 range for each accelerometer axis. When held at 45-degrees, rather than reading 500, I am getting closer to 650.
EDIT: I forgot my basic trig and a reading closer to 700 could correspond to 45 degrees, so I guess there is nothing wrong...nevermind
Are accelerometers normally non-linear, or should I be looking to use some function besides direct map() to calculate G readings?
Finally, here is the current code I'm working with, with my modifications. I am not the best with C, so please feel free to correct any bad programming practices.
// .......................................................................
// Code for reading data from a Wii Motion plus device connected to a Nunchuck
// Links to other sites
// http://www.windmeadow.com/node/42 .... first time i saw arduino nunchuk interface
// http://www.kako.com/neta/2009-017/2009-017.html# .....Japanese site with lots of Wii info
// http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus .... the one and only
// http://randomhacksofboredom.blogspot.com/2009/06/wii-motion-plus-arduino-love.html
// ....original motion plus but not passthrough
// http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1248889032/35 .... great Kalman filter code
// thanks to duckhead and knuckles904. I will be using that code for sure.
// http://obex.parallax.com/objects/471/ .... ideas for passthrough
// by Krulkip
// Here is the bitmapping which is changed from the standard nunchuk mapping
// In Nunchuk mode:
// Bit
// Byte 7 6 5 4 3 2 1 0
// 0 SX<7-----------------------------------------------------0>
// 1 SY<7-----------------------------------------------------0>
// 2 AX<9-----------------------------------------------------2>
// 3 AY<9-----------------------------------------------------2>
// 4 AZ<9---------------------------------------------3> 1
// 5 AZ<2-----1> AY<1> AX<1> BC BZ 0 0
// Please note also the loss of the bit0 resolution.
// Byte 5 Bit 0 and 1 is used for nunchuk (0 0) = nunchuk or motion plus (1 0) is motion plus detection.
// Byte 4 Bit 0 which is the extention controller detection bit. (1 = extension present)
// Hardware Arduino with ATMega 168
// Connections SCA to AD4 (Analog4) and SCL to AD5 (Analog5)
/*
Constants for scaling accelerometer values: Example of the accelerometer rotation values for y axis on Ardvark's nunchuk.
Exact values differ between x and y axis, and probably between nunchuks, but the range value may be constant:
298 ayMIN
_|_
/ \
ayMID 513 - | | - 513 ayMID
\ _ _ /
|
728 ayMAX
ayRange = (ayMID - ayMIN)*2 = ayMAX - ayMIN = 430
*/
//=========================================================
// changelog
// ---------
// 8/9/2009 - by dimkasta @ www.kastaniotis.net
// Adapted for use with the transistor-less switching code originally written by krulkip on
// http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1248889032/60
// Calibration and utility code based on the code by duckhead
// 9/9/2009 - by dimkasta @ www.kastaniotis.net
// Added Accelerometer calibration
// Removed the casting to double and the multiplying with millis/sec from the cycle saving to further greately
// increase the free time to ~1/50 in comparison to the reading time.
// 18/9/2009 - by Ardvark
// Slow/fast bit reading fixed: Slow degree/sec scale value appears correct.
// Included details for absolute scaling of accelerometer values.
// Included complementary filter for combining single accelerometer/gyro axes to produce drift- and translation-suppressed orientation values.
//=========================================================
// TODO LIST - Known issues
// ---------
// Assign common names to gyro and accel axes
//........................................................................
#include <Wire.h>
unsigned long Now = millis();
unsigned long lastread = Now;
unsigned long lastreadMP = Now;
//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.
boolean debug = true;
boolean calibrating = false;
uint8_t data[6]; // array to store arduino output
int cnt = 0;
int ledPin = 13;
int xID;
//wiibrew info seems to be incorrect with regards to scaling, see instead Xevel's comments: http://www.assembla.com/wiki/show/alicewiimotionplus/slow_and_fast_modes
static const int wmpSlowToDegreePerSec = 16; //when gyro speed 1, 16 units = 1°/s
static const int wmpFastToDegreePerSec = 4; //when gyro speed 0, 4 units = 0,25°/s
// Hardware identifiers. Polling data[5]&0x03 for present device
static const int NC = 0x00;
static const int MP = 0x02;
int currentDevice;
// WM+ state variables - if true, then in slow (high res) mode
boolean slowYaw = 0;
boolean slowPitch = 0;
boolean slowRoll = 0;
int rollScale;
int pitchScale;
int yawScale;
// Calibrated gyro values
int yaw = 0;
int pitch = 0;
int roll = 0;
float dt; // msec
float dtMP; // this is the time elapsed between readings of the motion plus (MP)
float rollCF = 0;
float pitchCF = 0;
static const double DegreeToRadian = 0.0174532925; // PI/180
// Complimentary filter: inspired by Shane Colton's description in filter.pdf at http://web.mit.edu/first/segway/segspecs.zip
// Tweak tau to alter the performance of the complimentary filter.
// Increase tau: influence of the gyro input increases (less translation artifacts).
// Reduce tau: influence of the accelerometer input increases (faster drift-correction).
// This filtering is temporal: translations that occur over a time scale shorter than tau will be suppressed,
// and gyro changes that occur over a time scale longer than tau will have their drift corrected.
// NB: changes in the read rate of the gyro (dtMP) will change the filter's performance (change tau to compensate).
static const float tau = 0.95; // unit: seconds.
float compFilter(float prevValue, int gyro, double accel, float timeStep) {
float a = tau/(tau + dtMP);
float b = 1 - a;
return (float) (a*(prevValue + ((gyro*DegreeToRadian)*timeStep)) + (b*accel));
}
// raw accelerometer values, no units
int ax_r = 0;
int ay_r = 0;
int az_r = 0;
// adjusted accelerometer readings, in G
float ax_g = 0;
float ay_g = 0;
float az_g = 0;
// accelerometer ranges, different per NK, experiment with raw values to find values for your own
// each max and min correspond to 1G and -1G for each axis, used to map() to +1 to -1 G for CF
#define ax_min 202
#define ax_max 570
#define ay_min 204
#define ay_max 570
#define az_min 184
#define az_max 566
// gyro calibration zeros
int yaw0 = 0;
int pitch0 = 0;
int roll0 = 0;
void setup ()
{
Serial.begin (115200);
Wire.begin();
initializeSensors();
calibrateZeroes();
}
void send_zero ()
{
Wire.beginTransmission(0x52);
Wire.write(0x00);
Wire.endTransmission();
}
void loop ()
{
Now = millis();
dt = Now - lastread; //compute the time delta since last iteration, in msec.
if (dt >= 10) //Only process delta angles if at least 1/100 of a second has elapsed. NB: 9 ms is the lowest possible interval before things jam up on an ATmega 328
{
readData();
lastread = Now;
}
else // just for demo of the free cycles
{
/*Serial.print("ax_r: ");
Serial.print(ax_r);
Serial.print(" ay_r: ");
Serial.print(ay_r);
Serial.print(" az_r: ");
Serial.println(az_r); */
Serial.print(" ax_g: ");
Serial.print(ax_g);
Serial.print(" ay_g: ");
Serial.print(ay_g);
Serial.print(" az_g: ");
Serial.println(az_g);
delay(50);
}
}
// split to fit in post