[sourcecode language=”plain”]</pre>
<div>
<div>//Includes</div>
<div>#include <Wire.h></div>
<div>//Gyro Variables</div>
<div>float elapsedTime, time, timePrev;        //Variables for time control</div>
<div>int gyro_error=0;                         //We use this variable to only calculate once the gyro data error</div>
<div>float Gyr_rawX, Gyr_rawY, Gyr_rawZ;     //Here we store the raw data read</div>
<div>float Gyro_angle_x, Gyro_angle_y;         //Here we store the angle value obtained with Gyro data</div>
<div>float Gyro_raw_error_x, Gyro_raw_error_y; //Here we store the initial gyro data error</div>
<div>//Acc Variables</div>
<div>int acc_error=0;                         //We use this variable to only calculate once the Acc data error</div>
<div>float rad_to_deg = 180/3.141592654;      //This value is for pasing from radians to degrees values</div>
<div>float Acc_rawX, Acc_rawY, Acc_rawZ;    //Here we store the raw data read</div>
<div>float Acc_angle_x, Acc_angle_y;          //Here we store the angle value obtained with Acc data</div>
<div>float Acc_angle_error_x, Acc_angle_error_y; //Here we store the initial Acc data error</div>
<div>float Total_angle_x, Total_angle_y;</div>
<div>void setup() {</div>
<div>  Wire.begin();                           //begin the wire comunication</div>
<div></div>
<div>  Wire.beginTransmission(0x68);           //begin, Send the slave adress (in this case 68)</div>
<div>  Wire.write(0x6B);                       //make the reset (place a 0 into the 6B register)</div>
<div>  Wire.write(0x00);</div>
<div>  Wire.endTransmission(true);             //end the transmission</div>
<div>  //Gyro config</div>
<div>  Wire.beginTransmission(0x68);           //begin, Send the slave adress (in this case 68)</div>
<div>  Wire.write(0x1B);                       //We want to write to the GYRO_CONFIG register (1B hex)</div>
<div>  Wire.write(0x10);                       //Set the register bits as 00010000 (1000dps full scale)</div>
<div>  Wire.endTransmission(true);             //End the transmission with the gyro</div>
<div>  //Acc config</div>
<div>  Wire.beginTransmission(0x68);           //Start communication with the address found during search.</div>
<div>  Wire.write(0x1C);                       //We want to write to the ACCEL_CONFIG register</div>
<div>  Wire.write(0x10);                       //Set the register bits as 00010000 (+/- 8g full scale range)</div>
<div>  Wire.endTransmission(true);</div>
<div>  Serial.begin(9600);                     //Remember to set this same baud rate to the serial monitor</div>
<div>  time = millis();                        //Start counting time in milliseconds</div>
<div>/*Here we calculate the acc data error before we start the loop</div>
<div> * I make the mean of 200 values, that should be enough*/</div>
<div>  if(acc_error==0)</div>
<div>  {</div>
<div>    for(int a=0; a<200; a++)</div>
<div>    {</div>
<div>      Wire.beginTransmission(0x68);</div>
<div>      Wire.write(0x3B);                       //Ask for the 0x3B register- correspond to AcX</div>
<div>      Wire.endTransmission(false);</div>
<div>      Wire.requestFrom(0x68,6,true);</div>
<div></div>
<div>      Acc_rawX=(Wire.read()<<8|Wire.read())/4096.0 ; //each value needs two registres</div>
<div>      Acc_rawY=(Wire.read()<<8|Wire.read())/4096.0 ;</div>
<div>      Acc_rawZ=(Wire.read()<<8|Wire.read())/4096.0 ;</div>
<div></div>
<div>      /*—X—*/</div>
<div>      Acc_angle_error_x = Acc_angle_error_x + ((atan((Acc_rawY)/sqrt(pow((Acc_rawX),2) + pow((Acc_rawZ),2)))*rad_to_deg));</div>
<div>      /*—Y—*/</div>
<div>      Acc_angle_error_y = Acc_angle_error_y + ((atan(-1*(Acc_rawX)/sqrt(pow((Acc_rawY),2) + pow((Acc_rawZ),2)))*rad_to_deg));</div>
<div></div>
<div>      if(a==199)</div>
<div>      {</div>
<div>        Acc_angle_error_x = Acc_angle_error_x/200;</div>
<div>        Acc_angle_error_y = Acc_angle_error_y/200;</div>
<div>        acc_error=1;</div>
<div>      }</div>
<div>    }</div>
<div>  }//end of acc error calculation</div>
<div>/*Here we calculate the gyro data error before we start the loop</div>
<div> * I make the mean of 200 values, that should be enough*/</div>
<div>  if(gyro_error==0)</div>
<div>  {</div>
<div>    for(int i=0; i<200; i++)</div>
<div>    {</div>
<div>      Wire.beginTransmission(0x68);            //begin, Send the slave adress (in this case 68)</div>
<div>      Wire.write(0x43);                        //First adress of the Gyro data</div>
<div>      Wire.endTransmission(false);</div>
<div>      Wire.requestFrom(0x68,4,true);           //We ask for just 4 registers</div>
<div></div>
<div>      Gyr_rawX=Wire.read()<<8|Wire.read();     //Once again we shif and sum</div>
<div>      Gyr_rawY=Wire.read()<<8|Wire.read();</div>
<div></div>
<div>      /*—X—*/</div>
<div>      Gyro_raw_error_x = Gyro_raw_error_x + (Gyr_rawX/32.8);</div>
<div>      /*—Y—*/</div>
<div>      Gyro_raw_error_y = Gyro_raw_error_y + (Gyr_rawY/32.8);</div>
<div>      if(i==199)</div>
<div>      {</div>
<div>        Gyro_raw_error_x = Gyro_raw_error_x/200;</div>
<div>        Gyro_raw_error_y = Gyro_raw_error_y/200;</div>
<div>        gyro_error=1;</div>
<div>      }</div>
<div>    }</div>
<div>  }//end of gyro error calculation</div>
<div>}//end of setup void</div>
<div>void loop() {</div>
<div>  timePrev = time;                        // the previous time is stored before the actual time read</div>
<div>  time = millis();                        // actual time read</div>
<div>  elapsedTime = (time – timePrev) / 1000; //divide by 1000 in order to obtain seconds</div>
<div>  //////////////////////////////////////Gyro read/////////////////////////////////////</div>
<div>    Wire.beginTransmission(0x68);            //begin, Send the slave adress (in this case 68)</div>
<div>    Wire.write(0x43);                        //First adress of the Gyro data</div>
<div>    Wire.endTransmission(false);</div>
<div>    Wire.requestFrom(0x68,4,true);           //We ask for just 4 registers</div>
<div></div>
<div>    Gyr_rawX=Wire.read()<<8|Wire.read();     //Once again we shif and sum</div>
<div>    Gyr_rawY=Wire.read()<<8|Wire.read();</div>
<div>    /*Now in order to obtain the gyro data in degrees/seconds we have to divide first</div>
<div>    the raw value by 32.8 because that's the value that the datasheet gives us for a 1000dps range*/</div>
<div>    /*—X—*/</div>
<div>    Gyr_rawX = (Gyr_rawX/32.8) – Gyro_raw_error_x;</div>
<div>    /*—Y—*/</div>
<div>    Gyr_rawY = (Gyr_rawY/32.8) – Gyro_raw_error_y;</div>
<div></div>
<div>    /*Now we integrate the raw value in degrees per seconds in order to obtain the angle</div>
<div>    * If you multiply degrees/seconds by seconds you obtain degrees */</div>
<div>    /*—X—*/</div>
<div>    Gyro_angle_x = Gyr_rawX*elapsedTime;</div>
<div>    /*—X—*/</div>
<div>    Gyro_angle_y = Gyr_rawY*elapsedTime;</div>
<div></div>
<div></div>
<div>  //////////////////////////////////////Acc read/////////////////////////////////////</div>
<div>  Wire.beginTransmission(0x68);     //begin, Send the slave adress (in this case 68)</div>
<div>  Wire.write(0x3B);                 //Ask for the 0x3B register- correspond to AcX</div>
<div>  Wire.endTransmission(false);      //keep the transmission and next</div>
<div>  Wire.requestFrom(0x68,6,true);    //We ask for next 6 registers starting withj the 3B</div>
<div>  /*We have asked for the 0x3B register. The IMU will send a brust of register.</div>
<div>  * The amount of register to read is specify in the requestFrom function.</div>
<div>  * In this case we request 6 registers. Each value of acceleration is made out of</div>
<div>  * two 8bits registers, low values and high values. For that we request the 6 of them</div>
<div>  * and just make then sum of each pair. For that we shift to the left the high values</div>
<div>  * register (<<) and make an or (|) operation to add the low values.</div>
<div>  If we read the datasheet, for a range of+-8g, we have to divide the raw values by 4096*/</div>
<div>  Acc_rawX=(Wire.read()<<8|Wire.read())/4096.0 ; //each value needs two registres</div>
<div>  Acc_rawY=(Wire.read()<<8|Wire.read())/4096.0 ;</div>
<div>  Acc_rawZ=(Wire.read()<<8|Wire.read())/4096.0 ;</div>
<div> /*Now in order to obtain the Acc angles we use euler formula with acceleration values</div>
<div> after that we substract the error value found before*/</div>
<div> /*—X—*/</div>
<div> Acc_angle_x = (atan((Acc_rawY)/sqrt(pow((Acc_rawX),2) + pow((Acc_rawZ),2)))*rad_to_deg) – Acc_angle_error_x;</div>
<div> /*—Y—*/</div>
<div> Acc_angle_y = (atan(-1*(Acc_rawX)/sqrt(pow((Acc_rawY),2) + pow((Acc_rawZ),2)))*rad_to_deg) – Acc_angle_error_y;</div>
<div> //////////////////////////////////////Total angle and filter/////////////////////////////////////</div>
<div> /*—X axis angle—*/</div>
<div> Total_angle_x = 0.98 *(Total_angle_x + Gyro_angle_x) + 0.02*Acc_angle_x;</div>
<div> /*—Y axis angle—*/</div>
<div> Total_angle_y = 0.98 *(Total_angle_y + Gyro_angle_y) + 0.02*Acc_angle_y;</div>
<div></div>
<div></div>
<div></div>
<div> /*Uncoment the rest of the serial prines</div>
<div> * I only print the Y angle value for this test */</div>
<div> Serial.print("Xº: ");</div>
<div> Serial.print(Total_angle_x);</div>
<div> Serial.print("   |   ");</div>
<div> Serial.print("Yº: ");</div>
<div> Serial.print(Total_angle_y);</div>
<div> Serial.println(" ");</div>
<div></div>
<div>}</div>
</div>
<pre>[/sourcecode]