 void start_servo(unsigned long current_millis) {
   myservo.write(SERVO_BASE_ANGLE);
   current_angle = 0;
+  current_max_angle = min(SERVO_MAX_ANGLE, get_max_angle());
   current_velocity_positive = true;
   previous_servo_move = current_millis;
-}
-
-void switch_servo() {
-  int num_steps = 4;
-  int base_angle = 90;
-  int step = SERVO_MAX_ANGLE / num_steps;
-
-  int interval = 60000 / current_bpm;
-  int delay_ms = interval * 4 / 5 / (num_steps * 2);
-
-  for (int i = 0; i < num_steps; ++i) {
-    myservo.write(base_angle + i * step);
-    delay(delay_ms);
-  }
-  for (int i = num_steps; i > 0; --i) {
-    myservo.write(base_angle + i * step);
-    delay(delay_ms);
-  }
-  myservo.write(base_angle);
 }
 
 int translate_ir() {

    default: return -1; // other button
  }
}

void test_speed(int angle) {
  for (int i = 0; i < 20; ++i) {
    myservo.write(90 - angle);
    delay(300);
    myservo.write(90 + angle);
    delay(300);
  }
}

  Serial.begin(9600);

  // initialize digital pin LED_BUILTIN as an output.
  // pinMode(LED_BUILTIN, OUTPUT);
  // pinMode(RED, OUTPUT);
  // pinMode(GREEN, OUTPUT);
  // pinMode(BLUE, OUTPUT);
  pinMode(LED_1, OUTPUT);
  pinMode(LED_2, OUTPUT);

  // digitalWrite(LED_BUILTIN, LOW);
  // digitalWrite(RED, LOW);
  // digitalWrite(GREEN, LOW);
  // digitalWrite(BLUE, LOW);
  digitalWrite(LED_1, LOW);
  digitalWrite(LED_2, LOW);

