void loop() {
  if (Serial.available() > 0) {
    short curr_status[] = { 35, 35, 35, 35, 110, 90, 1 }; # 之前速度值都是0，修改为35，避免电机启动不起来。
    Serial.readBytes((char*)curr_status, BUFFER_SIZE);

    short speeds[] = { curr_status[M0], curr_status[M1], curr_status[M2], curr_status[M3] };
    short angles[] = { curr_status[S1], curr_status[S2] };
    short check = curr_status[US];
    // Serial.println(curr_status[0]);
    if (!(check ^ check_val)) {
      if (stop) {
        Serial.println("FAIL");
      } else {
        set_motor(speeds);
        set_servo(angles);
        Serial.println("SUCC");
      }
    }
  }
}