ST_gram

Here's a bit of code for reading and displaying all 6 sensors:

//////////////////////////////////////////////////////////
// Simply reads the Razor 6DOF accelerometer/gyro and   //
// prints raw ADC values                                //
//////////////////////////////////////////////////////////

void setup(){
  analogReference(EXTERNAL);  // sets reference voltage to VREF
  Serial.begin(115200);
  delay(200);
}

void loop()
  {
  //Output the raw values
  Serial.print("Gyro x: ");
  Serial.print(analogRead(1));
  Serial.print(" | Gyro y: ");
  Serial.print(analogRead(0));
  Serial.print(" | Gyro z: ");
  Serial.print(analogRead(2));

  Serial.print("       Accel x: ");
  Serial.print(analogRead(5));
  Serial.print(" | Accel y: ");
  Serial.print(analogRead(4));
  Serial.print(" | Accel z: ");
  Serial.println(analogRead(3));

  delay(100);// slow down the serial output - Easier to read
}

If you use this code, just make sure to update the analogRead statements with your correct pins. With my board flat and close to level, I get the following 7 consecutive data readings: 

Gyro x: 379 | Gyro y: 380 | Gyro z: 377       Accel x: 504 | Accel y: 508 | Accel z: 617
Gyro x: 379 | Gyro y: 380 | Gyro z: 377       Accel x: 504 | Accel y: 508 | Accel z: 618
Gyro x: 378 | Gyro y: 380 | Gyro z: 378       Accel x: 504 | Accel y: 509 | Accel z: 619
Gyro x: 379 | Gyro y: 381 | Gyro z: 377       Accel x: 505 | Accel y: 509 | Accel z: 618
Gyro x: 379 | Gyro y: 380 | Gyro z: 377       Accel x: 504 | Accel y: 508 | Accel z: 618
Gyro x: 379 | Gyro y: 381 | Gyro z: 377       Accel x: 504 | Accel y: 508 | Accel z: 618
Gyro x: 379 | Gyro y: 380 | Gyro z: 377       Accel x: 504 | Accel y: 509 | Accel z: 618

Gyro_Rate = (Gyro_ADC_Value - Zero_Voltage_Value) * (3.3V/1023) / Gyro_Sensitivity

GyroRateX = (411-379) * (3.3/1023) / 0.00333  ----> (0.00333 is 3.33mV)
GyroRateX =  32 * 0.0032258 / 0.00333
GyroRateX = 30.999 deg/s

 GyroAngleX += GyroRateX * 0.02

Accel_X = (Accel_ADC_Value - Zero_Voltage_Value) * (3.3V/1023) / Accel_Sensitivity 

Accel_X = (558 - 504) * (3.3/1023) / 0.33 ----> (0.33 is 330mV)
Accel_X = 54 * 0.0032258 / 0.33
Accel_X = 0.528 G's

Accel_Angle_X = asin(Accel_X) * 57.2957795
Accel_Angle_X = asin(0.528) * 57.2957795
Accel_Angle_X = 0.556 * 57.2957795
Accel_Angle_X = 31.87 degrees

//The minimum and maximum values that came from
//the accelerometer...
//You very well may need to change these
int minValx = 403;
int maxValx = 610;

int minValy = 400;
int maxValy = 614;

int minValz = 413;
int maxValz = 619;

  //convert read values to degrees -90 to 90 - Needed for atan2
  int xAng = map(AN[3], minValx, maxValx, -90, 90);
  int yAng = map(AN[4], minValy, maxValy, -90, 90);
  int zAng = map(AN[5], minValz, maxValz, -90, 90);

  //Caculate 360deg values like so: atan2(-yAng, -zAng)
  //atan2 outputs the value of -π to π (radians)
  //We are then converting the radians to degrees
  AccAngleX = Rad2Deg * (atan2(-xAng, -zAng) + PI);
  AccAngleY = Rad2Deg * (atan2(-yAng, -zAng) + PI);

// These hold accelerometer maximum/minimum values
int aX_max=0, aX_min=0, aY_max=0, aY_min=0, aZ_max=0, aZ_min=0;

// Raw data variables
int aX, aY, aZ;


void setup()
{
  analogReference(EXTERNAL);  // sets reference voltage to use voltage applied to VREF pin
  Serial.begin(115200);
  
  delay(100);  // give some time for warm-up

  // Read and set starting values
  aX = analogRead(5);	// <- make sure to set your pins here
  aY = analogRead(4);
  aZ = analogRead(3); 
  
  aX_max = aX;
  aX_min = aX;
  aY_max = aY;
  aY_min = aY;
  aZ_max = aZ;
  aZ_min = aZ;
  
}

void loop()
{
  
  aX = analogRead(5);
  aY = analogRead(4);
  aZ = analogRead(3);  
  

aX_max = max(aX_max, aX);
aX_min = min(aX_min, aX);
aY_max = max(aY_max, aY);
aY_min = min(aY_min, aY);
aZ_max = max(aZ_max, aZ);
aZ_min = min(aZ_min, aZ);
 
  Serial.print("Accelerometer... axis(min,max):    X(");
  Serial.print(aX_min);
  Serial.print(",");
  Serial.print(aX_max);
  Serial.print(")  Y(");
  Serial.print(aY_min);
  Serial.print(",");
  Serial.print(aY_max);
  Serial.print(")  Z(");
  Serial.print(aZ_min);
  Serial.print(",");
  Serial.print(aZ_max);

  
    Serial.println(" ");
delay(100);
}

angle = (0.98)*(angle + gyro * dt) + (0.02)*(x_acc);

#define Gyro_Sens 0.00333 // Gyro sensitivity = 3.33mV/deg/s
#define VPQ 0.00322581 // Volts Per Quid --- value of 3.3V/1023
#define ADC_Avg_Num  100.// Number of averaging readings for calibration
#define Rad2Deg 57.2957795 // 1 radian = 57.2957795 degrees
#define Deg2Rad 0.0174532925 // 0.0174532925 rads = 1 deg

//The minimum and maximum values that came from
//the accelerometer...
//You very well may need to change these
int minValx = 403;
int maxValx = 610;

int minValy = 401;
int maxValy = 614;

int minValz = 413;
int maxValz = 619;

//Calibration variables
float Gx_Cal, Gy_Cal, Gz_Cal;

float GyroRateX=0, GyroRateY=0, GyroAngleZ=0, GyroAngleZ_dt=0;
float AccAngleX=0, AccAngleY=0, AccAngleZ=0;

float Pitch, Yaw, Roll;

int AN[6];  // Hold analogRead data

unsigned long pre_time, print_clock=0;
float dtime;


void setup()
{
  analogReference(EXTERNAL);  // sets reference voltage to use voltage applied to VREF pin
  Serial.begin(115200);
  delay(300);  // Give things time to "warm-up"

  Calibrate(); // Calibrate sensors
  pre_time = millis(); //store current time to be used as "previous" time
}



void loop()
{

  if(millis()-pre_time>=20) // Read ADC and does Calculations every 20ms
  {
    dtime=millis()-pre_time;  //current time - previous time
    pre_time = millis(); //store current time to be used as "previous" time
    dtime=dtime/1000.;

    Read_ADC();
    Calculate();
  }


  if(millis()-print_clock>=50)  //print every 50ms
  {
  Serial.print(Pitch);
  Serial.print(",");
  Serial.print(Roll);
  Serial.print(",");
  Serial.println(Yaw);
  
  print_clock=millis();  // store current time
  
  } //end if


}





///////////////////////////////////////////////////////////////////////
////////////////////////   Functions    ///////////////////////////////
///////////////////////////////////////////////////////////////////////



void Read_ADC(void)
{

  AN[0] = analogRead(1); // Gyro_X
  AN[1] = analogRead(0); // Gyro_Y
  AN[2] = analogRead(2); // Gyro_Z
  AN[3] = analogRead(5); // Acc_X
  AN[4] = analogRead(4); // Acc_Y
  AN[5] = analogRead(3); // Acc_Z
}



void Calculate(void)
{

// Gyro portion
//----------------------------------------------------------------
  GyroRateX = -1.0*dtime * (((AN[0]*3.3)/1023.-Gx_Cal)/Gyro_Sens);
  GyroRateY = dtime * (((AN[1]*3.3)/1023.-Gy_Cal)/Gyro_Sens);
  
  GyroAngleZ_dt = dtime * (((AN[2]*3.3)/1023.-Gz_Cal)/Gyro_Sens);
  GyroAngleZ += -1.0 * GyroAngleZ_dt * (1/(cos(Deg2Rad*Roll))); // convert Roll angle to Rads, find sin to use as scaler for Yaw

  if(GyroAngleZ<0) GyroAngleZ+=360;    // Keep within range of 0-360 deg
  if(GyroAngleZ>=360) GyroAngleZ-=360;
//----------------------------------------------------------------

  //convert read values to degrees -90 to 90 - Needed for atan2
  int xAng = map(AN[3], minValx, maxValx, -90, 90);
  int yAng = map(AN[4], minValy, maxValy, -90, 90);
  int zAng = map(AN[5], minValz, maxValz, -90, 90);

  //Caculate 360deg values like so: atan2(-yAng, -zAng)
  //atan2 outputs the value of -π to π (radians)
  //We are then converting the radians to degrees
  AccAngleX = Rad2Deg * (atan2(-xAng, -zAng) + PI);
  AccAngleY = Rad2Deg * (atan2(-yAng, -zAng) + PI);

  // Keep angles between +-180deg
  if(AccAngleX>180) AccAngleX=AccAngleX-360;

  if(AccAngleY<=180) AccAngleY=-1.0*AccAngleY;
  if(AccAngleY>180) AccAngleY=360-AccAngleY;

  // Final values...
  Roll = (0.98)*(Roll + GyroRateX) + (0.02)*(AccAngleX);
  Pitch = (0.98)*(Pitch + GyroRateY) + (0.02)*(AccAngleY);
  Yaw = GyroAngleZ;
}


// Reads and averages ADC values for calibration
float Avg_ADC(int ADC_In)
{
  long ADC_Temp=0;
  for(int i=0; i<ADC_Avg_Num; i++)
  {
    ADC_Temp = ADC_Temp+analogRead(ADC_In);
    delay(10); // Delay 10ms due to gyro bandwidth limit of 140Hz (~7.1ms)
  }
  return VPQ*(ADC_Temp/ADC_Avg_Num); //Average ADC, convert to volts
}

void Calibrate(void)
{
  Gx_Cal = Avg_ADC(1); // Gyro_x on pin 1
  Gy_Cal = Avg_ADC(0); // Gyro_y on pin 0
  Gz_Cal = Avg_ADC(2); // Gyro_z on pin 2
}

////////////////////////
//////// END CODE //////
////////////////////////