 
 #include <Wire.h>
 #include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
+#include <DueTimer.h>
 
 #define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
 

int control_decision = 0;
double error = 0;
double delta_error = 0;


void setup() {

}

void loop() {
  digitalWrite(5, LOW);
  Serial.print(motor_speed * A_trend_first);
  Serial.print(' ');
  Serial.println(motor_speed2 * A_trend_first2);
  digitalWrite(5, HIGH);
  /* Update all the values */
  while (i2cRead(0x3B, i2cData, 14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
