Серводвигатель не будет работать для определенного кода

Я написал код с прерываниями для управления сервоприводом, но мои сервоприводы не работают. Один предназначен для работы с прерыванием, но другой должен просто двигаться, но он также не работает. Даже последовательное окно показывает, что код работает правильно, но сервоприводы не двигаются, я проверил оба моих сервопривода с одинаковыми соединениями и примером развертки Arduino, и оба работают нормально.

#include <TimerOne.h>                                 // Header file for TimerOne library
#include <Servo.h>
#define trigPin 12                                    // Pin 12 trigger output
#define echoPin 2                                     // Pin 2 Echo input

#define echo_int 0                                    // Interrupt id for echo pulse

#define TIMER_US 50                                   // 50 uS timer duration 
#define TICK_COUNTS 4000                              // 200 mS worth of timer ticks

volatile long echo_start = 0;                         // Records start of echo pulse 
volatile long echo_end = 0;                           // Records end of echo pulse
volatile long echo_duration = 0;                      // Duration - difference between end and start
volatile int trigger_time_count = 0;                  // Count down counter to trigger pulse time
volatile long range_flasher_counter = 0;              // Count down counter for flashing distance LED


int sound = 250;

Servo servo1; //Servos
Servo servo2;


const int button1 = 10; //Buttons
const int button2 = 8;
const int button3 = 13;
const byte interruptPin = 3;
int pos;


void setup() {
  servo1.attach(9); // servo for arm
  servo2.attach(5); // servo for base
  pinMode(trigPin, OUTPUT);                           // Trigger pin set to output
  pinMode(echoPin, INPUT);                            // Echo pin set to input
                      // Onboard LED pin set to output
  
  Timer1.initialize(TIMER_US);                        // Initialise timer 1
  Timer1.attachInterrupt( timerIsr );                 // Attach interrupt to the timer service routine 
  attachInterrupt(echo_int, echo_interrupt, CHANGE);
  pinMode(interruptPin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(interruptPin),Metal_detected, HIGH);

  pinMode(button1, INPUT);
  pinMode(button2, INPUT);
  Serial.begin(9600);
}

void loop() {
  // put your main code here, to run repeatedly: 


  servo1.write(0); // These will make the servos move to the mapped angles
  servo2.write(90);
  
  distance_checking();
  if(digitalRead(button1) == HIGH)
  {
   
      while(digitalRead(button2) == LOW)
        {
          Serial.println("Entering Sweeping mode");
       for (pos = 30; pos <= 150; pos += 1)
       { Serial.print("Angle is :");
         Serial.println(pos);
         servo2.write(pos);              
         distance_checking();
         //delay(0.1);                       // waits 15ms for the servo to reach the position
         if(digitalRead(button2) == HIGH)
          { 
            Serial.print("Exiting Sweeping");
            goto label;}
          }
          for (pos = 150; pos >= 30; pos -= 1) { // goes from 180 degrees to 0 degree
          Serial.print("Angle is :");
          Serial.println(pos);
          servo2.write(pos);              // tell servo to go to position in variable 'pos'
          distance_checking();
          //delay(0.1);                       // waits 15ms for the servo to reach the position
          if(digitalRead(button2) == HIGH)
          {
            goto label;
            Serial.print("Exiting Sweeping");}
          }

 }
 }
//reset th
label:
  if(digitalRead(button2) == HIGH){ 
    
    servo1.write(0);
    servo2.write(90);
    Serial.println("press the sweeping button to enter sweeeping mode");
    delay(300);
  }

} 


void distance_checking()
{ 

if (echo_duration/58 <= 20) 
{
Serial.println("the servo angle is 30");
servo1.write(30);
delay(1500);
}

else {
servo1.write(0);
}

delay(500);
}





void Metal_detected()
{if(digitalRead(button2) == LOW)
{delay(5000);
  Serial.print("Metal detected at servo angle:");
Serial.println(servo2.read());
servo1.write(servo1.read());
servo2.write(servo2.read());
Serial.println("Motion is stopped");
Serial.println("Press reset to go to the home position");


}
//while(digitalRead(button2) == HIGH)
// {
//  Serial.print("Reseting");
//  return 0;}
 }
void timerIsr()
{
    trigger_pulse();                                 // Schedule the trigger pulses
                             // Flash the onboard LED distance indicator
}

// --------------------------
// trigger_pulse() called every 50 uS to schedule trigger pulses.
// Generates a pulse one timer tick long.
// Minimum trigger pulse width for the HC-SR04 is 10 us. This system
// delivers a 50 uS pulse.
// --------------------------
void trigger_pulse()
{
      static volatile int state = 0;                 // State machine variable

      if (!(--trigger_time_count))                   // Count to 200mS
      {                                              // Time out - Initiate trigger pulse
         trigger_time_count = TICK_COUNTS;           // Reload
         state = 1;                                  // Changing to state 1 initiates a pulse
      }
    
      switch(state)                                  // State machine handles delivery of trigger pulse
      {
        case 0:                                      // Normal state does nothing
            break;
        
        case 1:                                      // Initiate pulse
           digitalWrite(trigPin, HIGH);              // Set the trigger output high
           state = 2;                                // and set state to 2
           break;
        
        case 2:                                      // Complete the pulse
        default:      
           digitalWrite(trigPin, LOW);               // Set the trigger output low
           state = 0;                                // and return state to normal 0
           break;
     }
}

// --------------------------
// echo_interrupt() External interrupt from HC-SR04 echo signal. 
// Called every time the echo signal changes state.
//
// Note: this routine does not handle the case where the timer
//       counter overflows which will result in the occassional error.
// --------------------------
void echo_interrupt()
{
  switch (digitalRead(echoPin))                     // Test to see if the signal is high or low
  {
    case HIGH:                                      // High so must be the start of the echo pulse
      echo_end = 0;                                 // Clear the end time
      echo_start = micros();                        // Save the start time
      break;
      
    case LOW:                                       // Low so must be the end of hte echo pulse
      echo_end = micros();                          // Save the end time
      echo_duration = echo_end - echo_start;        // Calculate the pulse duration
      break;
  }
}

Я протестировал свои двигатели с этим кодом один за другим, и они работали абсолютно нормально:

#include <Servo.h>

Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards

int pos = 0;    // variable to store the servo position

void setup() {
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
}

void loop() {
  for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
  for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
}