     Serial.println();
   }
 
-  // =========================================== 3. angle pid control ============================================== //
+  // =========================================== 5. angle pid control ============================================== //
   robot_angle = kalAngleX;
   error = robot_angle - target_angle;
   delta_error = error - last_error;


// angle PID control definitions
double current_angle = 0;
double target_angle = 7;
double error = 0;
double last_error = 0;
double delta_error = 0;

void timeOut2Handler() {
  if (micros() - last_time2 > 5000) motor_speed2 = 0;
}
// ========================================= don't see above section =======================================//

void receive_info() {
  int raw_command = Serial.read() - 49;
  // Integrity checking
  if (raw_command < idle || raw_command > right_turn) {
    Serial.print("Fail state");
    return;
  }
  command = (state)raw_command;
  Serial.print(command);
}

void setup() {
  Serial.begin(115200);
  digitalWrite(8,LOW);
  pinMode(21, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);
