     delayMicroseconds(3);
   }
   pinMode(21, INPUT);
+  pinMode(5, OUTPUT);
+  Timer.getAvailable().attachInterrupt(timeOut1Handler).start(1000);
+  attachInterrupt(digitalPinToInterrupt(50), enA_rise_ISR1, RISING);
+  attachInterrupt(digitalPinToInterrupt(51), enA_fall_ISR1, FALLING);
+  attachInterrupt(digitalPinToInterrupt(52), enB_rise_ISR1, RISING);
+  attachInterrupt(digitalPinToInterrupt(53), enB_fall_ISR1, FALLING);
+  Timer.getAvailable().attachInterrupt(timeOut2Handler).start(1000);
+  attachInterrupt(digitalPinToInterrupt(22), enA_rise_ISR2, RISING);
+  attachInterrupt(digitalPinToInterrupt(23), enA_fall_ISR2, FALLING);
+  attachInterrupt(digitalPinToInterrupt(24), enB_rise_ISR2, RISING);
+  attachInterrupt(digitalPinToInterrupt(25), enB_fall_ISR2, FALLING);
   Wire.begin();
 
   i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz

int dirB = 13;
int test = 6;
double current_angle = 0;
double target_angle = 2.8;
int moving_factor = 0;
double Kp = 45;
double Kd = 225;
double Ki = 0.45;
int integrated_error;
int p1, p2;
double robot_angle;

}

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]);
