Servo motor won't work for a specific code - interrupt

Question

I have written a code with interrupts to control the servo but my servos are not working. One is meant to work with interrupt but the other one has to simply move but it is also not working. Even the serial window shows that the code is working properly ut the servos aren't moving I have checked both my servos with same connections and Sweep example of Arduino and both work fine.
#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;
}
}
I tested my motors with this code one by one and they worked absolutely fine:
#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
}
}