Hi Lauszus,
First - thank you for sharing your work!
I picked up a 6DOF razor and thought I'd try your code before I dug into it...
I'm getting nothing from the serial port running IMU6DOF2. (Arduino 022, 168-based board)
I confirmed connections are correct. processing(); is not commented out. It compiles and uploads fine, then sits like a bump on a log.
So I stripped it down to the essentials. I get some data, but I'm not sure it's sensible. Here's "the essentials" and the datastream I get:
//made by Kristian Lauszus - see http://arduino.cc/forum/index.php/topic,58048.0.html for information
#define gX A0
#define gY A1
#define gZ A2
#define aX A3
#define aY A4
#define aZ A5
#define RAD_TO_DEG 57.295779513082320876798154814105
//gyros
int gyroZeroX;//x-axis
float gyroXadc;
float gyroXrate;
float gyroXangle;
int gyroZeroY;//y-axis
float gyroYadc;
float gyroYrate;
float gyroYangle;
int gyroZeroZ;//z-axis
float gyroZadc;
float gyroZrate;
float gyroZangle;
//accelerometers
int accZeroX;//x-axis
float accXadc;
float accXval;
float accXangle;
int accZeroY;//y-axis
float accYadc;
float accYval;
float accYangle;
int accZeroZ;//z-axis
float accZadc;
float accZval;
float accZangle;
//Results
float xAngle;
float yAngle;
float compAngleX;
float compAngleY;
float R;//force vector
//Used for timing
unsigned long timer=0;
unsigned long dtime=0;
void setup()
{
analogReference(EXTERNAL); //3.3V
Serial.begin(115200);
delay(100);//wait for the sensor to get ready
timer=millis();//start timing
}
void loop()
{
gyroXadc = analogRead(gX);
gyroXrate = (gyroXadc-gyroZeroX)/1.0323;//(gyroXadc-gryoZeroX)/Sensitivity - in quids Sensitivity = 0.00333/3.3*1023=1.0323
gyroXangle=gyroXangle+gyroXrate*dtime/1000;//Without any filter
gyroYadc = analogRead(gY);
gyroYrate = (gyroYadc-gyroZeroY)/1.0323;//(gyroYadc-gryoZeroX)/Sensitivity - in quids Sensitivity = 0.00333/3.3*1023=1.0323
gyroYangle=gyroYangle+gyroYrate*dtime/1000;//Without any filter
gyroZadc = analogRead(gZ);
gyroZrate = (gyroZadc-gyroZeroZ)/1.0323;//(gyroZadc-gryoZeroX)/Sensitivity - in quids Sensitivity = 0.00333/3.3*1023=1.0323
//gyroZangle=gyroZangle+gyroZrate*dtime/1000;//Without any filter
accXadc = analogRead(aX);
accXval = (accXadc-accZeroX)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids Sensitivity = 0.33/3.3*1023=102,3
accYadc = analogRead(aY);
accYval = (accYadc-accZeroY)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids Sensitivity = 0.33/3.3*1023=102,3
accZadc = analogRead(aZ);
accZval = (accZadc-accZeroZ)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids Sensitivity = 0.33/3.3*1023=102,3
accZval++;//1g in horizontal position
R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector
accXangle = acos(accXval/R)*RAD_TO_DEG-90;
accYangle = acos(accYval/R)*RAD_TO_DEG-90;
//accZangle = acos(accZval/R)*RAD_TO_DEG;
//used for debugging
Serial.print(gyroXrate,0);Serial.print("\t");
Serial.print(gyroYrate,0);Serial.print("\t");
Serial.print(gyroZrate,0);Serial.print("\t");
Serial.print(gyroXangle,0);Serial.print("\t");
Serial.print(gyroYangle,0);Serial.print("\t");
Serial.print(gyroZangle,0);Serial.print("\t");
Serial.print(accXval,2);Serial.print("\t");
Serial.print(accYval,2);Serial.print("\t");
Serial.print(accZval,2);Serial.print("\t");
Serial.print(accXangle,0);Serial.print("\t");
Serial.print(accYangle,0);Serial.print("\t");
//Serial.print(accZangle,0);Serial.print("\t");
Serial.print("\n");
dtime = millis()-timer;
timer = millis();
}
Here's a sample of the data sitting on my desk:
159 363 382 9184 9231 0 5.70 5.04 6.60 -34 -30
157 368 378 9185 9234 0 5.72 5.01 6.59 -35 -30
161 375 374 9186 9236 0 5.68 4.98 6.61 -34 -30
167 384 367 9187 9239 0 5.61 5.00 6.66 -34 -30
170 396 359 9188 9241 0 5.57 5.06 6.68 -34 -30
170 410 354 9189 9244 0 5.58 5.08 6.61 -34 -30
175 418 351 9190 9246 0 5.63 5.09 6.49 -34 -31
181 422 351 9192 9249 0 5.68 5.11 6.45 -35 -31
187 425 354 9193 9252 0 5.71 5.14 6.40 -35 -31
192 426 358 9194 9254 0 5.75 5.16 6.32 -35 -31
I mention that I'm not sure it's sensible (and that may just be due to a lack of calibration?) because, if we look at the last line:
gyroXrate: 192
gyroYrate: 426
gyroZrate: 358
gyroXangle: 9194 [!!!]
gyroYangle: 9254
gyroZangle: 0
AccX: 5.75
AccY: 5.16
AccZ: 6.32
AccXAngle: -35
AccYAngle: -31
Clearly that looks "a little funny"...
If you'd like to compare the project I'm running (entirely unchanged), I pushed the zip file here:
http://www.jlrdesigns.com/IMU6DOFVer2.zip
Thanks for any advice!