#include <Servo.h>

//Ultrasonic sensor without pulsein
unsigned long Ttrig; //Time of trigger
unsigned long Techo; //Time of echo
int trigger_pin = 12;
int echo_pin = 3;
int FSM_ultrasonic_state = 0;
byte echoRead = 0;
float distance;
unsigned long duration = 0;
int reset_echoRead = 0;
//Ultrasonic sensor without pulsein

//SERVO pin Config
int s0 = 4;
int s1 = 5;
int s2 = 6;
int s3 = 7;
int s4 = 8;


//SERVO START POSITION
int servo0_start = 90; //90
int servo1_start = 105;
int servo2_start = 85;
int servo3_start = 60;
int servo4_start = 100;


const byte interruptPin = 2;
const byte startButton = 13;
byte FSM_global_is_actual = 0;
byte FSM_global_is_before = 0;
int stimulus_interrupt = 1;

//FSM Obstacle
byte FSM_Obstacle_state = 0;

//NUMBER OF STEPS in each loop CYCLE
byte stepNumber = 1;

int servo_delay = 30;

//FSM GLOBAL VALs
int FSM_GLOBAL_servo0_VAL;
int FSM_GLOBAL_servo1_VAL;
int FSM_GLOBAL_servo2_VAL;
int FSM_GLOBAL_servo3_VAL;
int FSM_GLOBAL_servo4_VAL;

//FSM Servo 0
Servo servo0; //servo 0 definition
byte FSM_servo0_state = 0;
int FSM_servo0_AP = servo0_start;//to avoid erratic movement
int FSM_servo0_FP = servo0_start;
int FSM_servo0_VAL = 0;
unsigned long ts_servo0;

//FSM Servo 1
Servo servo1; //servo 1 definition
byte FSM_servo1_state = 0;
int FSM_servo1_AP = servo1_start;//to avoid erratic movement
int FSM_servo1_FP = servo1_start;
int FSM_servo1_VAL = 0;
unsigned long ts_servo1;

//FSM Servo 2
Servo servo2; //servo 2 definition
byte FSM_servo2_state = 0;
int FSM_servo2_AP = servo2_start;//to avoid erratic movement
int FSM_servo2_FP = servo2_start;
int FSM_servo2_VAL = 0;
unsigned long ts_servo2;

//FSM Servo 3
Servo servo3; //servo 3 definition
byte FSM_servo3_state = 0;
int FSM_servo3_AP = servo3_start;//to avoid erratic movement
int FSM_servo3_FP = servo3_start;
int FSM_servo3_VAL = 0;
unsigned long ts_servo3;

//FSM Servo 4
Servo servo4; //servo 4 definition
byte FSM_servo4_state = 0;
int FSM_servo4_AP = servo4_start;//to avoid erratic movement
int FSM_servo4_FP = servo4_start;
int FSM_servo4_VAL = 0;
unsigned long ts_servo4;

// FSM GLOBAL
byte FSM_global_state = 0;


////Setup
void setup() {
  Serial.begin(9600);
  pinMode(startButton, INPUT);
  pinMode(interruptPin, OUTPUT); //Interrupt associated with the Obstacle FSM
  
  servo0.attach(s0); // attach Servo 0 to pin
  servo1.attach(s1); // attach Servo 1 to pin
  servo2.attach(s2); // attach Servo 2 to pin
  servo3.attach(s3); // attach Servo 3 to pin
  servo4.attach(s4); // attach Servo 4 to pin
  servo0.write(servo0_start); //inicializing positions equal to first position in Global FSM.
  servo1.write(servo1_start); //This is to prevent erratic movement of the servos
  servo2.write(servo2_start);
  servo3.write(servo3_start);
  servo4.write(servo4_start);
                
  digitalWrite(interruptPin, LOW); // RESET Interrupt
  attachInterrupt(digitalPinToInterrupt(interruptPin), call_interrupt, CHANGE);

  //Sensor ultrasonico no pulsein
  pinMode(trigger_pin, OUTPUT);

  attachInterrupt(digitalPinToInterrupt(echo_pin), getTime, FALLING);
  //Sensor ultrasonico no pulsein
  
}
////END setup

////loop
void loop() {
  
  duration = Techo - Ttrig;
    
  if(reset_echoRead && echoRead){
    echoRead = 0;
  }

  int stimulus = 0;
  FSM_ultrasonic(echoRead, reset_echoRead); //FSM_ultrasonic; measures distance
  if (distance<20 && distance>2){ //checks if distance is within the interval
    stimulus = 1;
  }
  FSM_Obstacle(stimulus); //FSM_Obstacle

  //Stimulus for FSM global
  if (FSM_global_is_actual == 1 && FSM_global_is_before == 0){
    stimulus_interrupt = 0;
    } else if (FSM_global_is_actual == 0 && FSM_global_is_before == 1){
    stimulus_interrupt = 1;
    }
    
  FSM_global(digitalRead(startButton), stimulus_interrupt); //FSM global
  
  FSM_servo0(FSM_servo0_FP, FSM_GLOBAL_servo0_VAL); //FSM_servo0
  FSM_servo1(FSM_servo1_FP, FSM_GLOBAL_servo1_VAL); //FSM_servo1
  FSM_servo2(FSM_servo2_FP, FSM_GLOBAL_servo2_VAL); //FSM_servo2
  FSM_servo3(FSM_servo3_FP, FSM_GLOBAL_servo3_VAL); //FSM_servo3
  FSM_servo4(FSM_servo4_FP, FSM_GLOBAL_servo4_VAL); //FSM_servo4
}
////END loop


void call_interrupt() { //Interrupt activated by FSM_obstacle
  if(FSM_global_is_actual){
    FSM_global_is_actual = 0;
    FSM_global_is_before = 1;
    } else {
      FSM_global_is_actual = 1;
      FSM_global_is_before = 0;
      }
}


///// FSM GLOBAL - Controls all servos
void FSM_global(int button, int interrupt){
  FSM_global_nextstate(button,interrupt);
  FSM_global_output();
}

void FSM_global_nextstate(int button, int interrupt){
  switch(FSM_global_state){
    case 0:
      if(button){
        FSM_global_state = 1;
      }
    break;
    case 1:
    //POS 1
      if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL*FSM_servo4_VAL){
        FSM_global_state = 2;
      }
    break;
    case 2:
      FSM_global_state = 3;
    break;
    case 3:
    //POS 2
      if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL*FSM_servo4_VAL){
        FSM_global_state = 4;
      }
    break;
    case 4:
      FSM_global_state = 5;
    break;
    case 5:
    //POS 3 Hand opens
      if(FSM_servo4_VAL){
        FSM_global_state = 6;
      }
    break;
    case 6:
      FSM_global_state = 7;
    break;
    case 7:
    //POS 4 Hand closes
      if(FSM_servo4_VAL){
        FSM_global_state = 8;
      }
    break;
    case 8:
      FSM_global_state = 9;
    break;
    case 9:
    //POS 5 = POS 1 => POS A
      if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL){
        FSM_global_state = 10;
      }
    break;
    case 10:
      FSM_global_state = 11;
    break;
    case 11:
    //POS 6 = POS 1 with servo0 different => POS B
      if(interrupt) FSM_global_state = 20; // POS 6
      if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL){
        FSM_global_state = 12;
      }
    break;
    case 12:
      FSM_global_state = 13;
    break;
    case 13:
    //POS 7 = POS 2 with servo0 different
      if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL){
        FSM_global_state = 14;
      }
    break;
    case 14:
      FSM_global_state = 15;
    break;
    case 15:
    //POS 8 Hand opens
      if(FSM_servo4_VAL){
        FSM_global_state = 16;
      }
    break;
    case 16:
      FSM_global_state = 0;
    break;
    case 20: //state for search alternative Z position upward
      if (!interrupt) FSM_global_state = 10;
      if(FSM_servo2_VAL){
      FSM_global_state = 21;
      }
    break;
    case 21:
      FSM_global_state = 22;
    break;
    case 22: //state for search alternative Z position downward
      if (!interrupt) FSM_global_state = 10;
      if(FSM_servo2_VAL){ 
      FSM_global_state = 23;
      }
    break;
    case 23:
      FSM_global_state = 20;
    break;
  
  }
}

void FSM_global_output(){
    switch(FSM_global_state){
    case 0:
      //waiting to start
      FSM_servo0_FP = servo0_start;
      FSM_servo1_FP = servo1_start;
      FSM_servo2_FP = servo2_start;
      FSM_servo3_FP = servo3_start;
      FSM_servo4_FP = servo4_start;
      FSM_GLOBAL_servo0_VAL = 1;
      FSM_GLOBAL_servo1_VAL = 1;
      FSM_GLOBAL_servo2_VAL = 1;
      FSM_GLOBAL_servo3_VAL = 1;
      FSM_GLOBAL_servo4_VAL = 1;
    break;
    case 1:
    //POS 1
      FSM_servo0_FP = 170;
      FSM_servo1_FP = 130;
      FSM_servo2_FP = 30;
      FSM_servo3_FP = 25;
      FSM_GLOBAL_servo0_VAL = 0;
      FSM_GLOBAL_servo1_VAL = 0;
      FSM_GLOBAL_servo2_VAL = 0;
      FSM_GLOBAL_servo3_VAL = 0;
      FSM_GLOBAL_servo4_VAL = 0;
    break;
    case 2:
      FSM_GLOBAL_servo0_VAL = 1;
      FSM_GLOBAL_servo1_VAL = 1;
      FSM_GLOBAL_servo2_VAL = 1;
      FSM_GLOBAL_servo3_VAL = 1;
    break;
    case 3:
    //POS 2
      FSM_servo0_FP = 170;
      FSM_servo1_FP = 65;
      FSM_servo2_FP = 5;
      FSM_servo3_FP = 140;
      FSM_GLOBAL_servo0_VAL = 0;
      FSM_GLOBAL_servo1_VAL = 0;
      FSM_GLOBAL_servo2_VAL = 0;
      FSM_GLOBAL_servo3_VAL = 0;
    break;
    case 4:
      FSM_GLOBAL_servo0_VAL = 1;
      FSM_GLOBAL_servo1_VAL = 1;
      FSM_GLOBAL_servo2_VAL = 1;
      FSM_GLOBAL_servo3_VAL = 1;
      FSM_GLOBAL_servo4_VAL = 1;
    break;
    case 5:
    //POS 3 Hand opens
      FSM_servo4_FP = 15;
      FSM_GLOBAL_servo4_VAL = 0;
    break;
    case 6:
      FSM_GLOBAL_servo4_VAL = 1;
    break;
    case 7:
    //POS 4 Hand closes
      FSM_servo4_FP = 100;
      FSM_GLOBAL_servo4_VAL = 0;
    break;
    case 8:
      FSM_GLOBAL_servo0_VAL = 1;
      FSM_GLOBAL_servo1_VAL = 1;
      FSM_GLOBAL_servo2_VAL = 1;
      FSM_GLOBAL_servo3_VAL = 1;
      FSM_GLOBAL_servo4_VAL = 1;
    break;
    case 9:
    //POS 5 = POS 1 => POS A
      FSM_servo0_FP = 170;
      FSM_servo1_FP = 130;
      FSM_servo2_FP = 30;
      FSM_servo3_FP = 25;
      FSM_GLOBAL_servo0_VAL = 0;
      FSM_GLOBAL_servo1_VAL = 0;
      FSM_GLOBAL_servo2_VAL = 0;
      FSM_GLOBAL_servo3_VAL = 0;
    break;
    case 10:
      FSM_GLOBAL_servo0_VAL = 1;
      FSM_GLOBAL_servo1_VAL = 1;
      FSM_GLOBAL_servo2_VAL = 1;
      FSM_GLOBAL_servo3_VAL = 1;
      FSM_servo2_FP = FSM_servo2_AP;//keeps z position after obstacle avoidance
    break;
    case 11:
    //POS 6 = POS 1 with servo0 different => POS B
      FSM_servo0_FP = 30;
      FSM_servo1_FP = 130;
      FSM_servo3_FP = 25;
      FSM_GLOBAL_servo0_VAL = 0;
      FSM_GLOBAL_servo1_VAL = 0;
      FSM_GLOBAL_servo2_VAL = 0;
      FSM_GLOBAL_servo3_VAL = 0;
    break;
    case 12:
      FSM_GLOBAL_servo0_VAL = 1;
      FSM_GLOBAL_servo1_VAL = 1;
      FSM_GLOBAL_servo2_VAL = 1;
      FSM_GLOBAL_servo3_VAL = 1;
    break;
    case 13:
    //POS 7 = POS 2 with servo0 different
      FSM_servo0_FP = 30;
      FSM_servo1_FP = 65;
      FSM_servo2_FP = 5;
      FSM_servo3_FP = 140;
      FSM_GLOBAL_servo0_VAL = 0;
      FSM_GLOBAL_servo1_VAL = 0;
      FSM_GLOBAL_servo2_VAL = 0;
      FSM_GLOBAL_servo3_VAL = 0;
    break;
    case 14:
      FSM_GLOBAL_servo0_VAL = 1;
      FSM_GLOBAL_servo1_VAL = 1;
      FSM_GLOBAL_servo2_VAL = 1;
      FSM_GLOBAL_servo3_VAL = 1;
    break;
    case 15:
    //POS 8 Hand opens
      FSM_servo4_FP = 15;
      FSM_GLOBAL_servo4_VAL = 0;
    break;
    case 16:
      FSM_GLOBAL_servo4_VAL = 1;
    break;
    case 20: //state for search alternative Z position upward
      FSM_servo2_FP = 60;
      FSM_servo0_FP = FSM_servo0_AP;
      FSM_servo1_FP = FSM_servo1_AP;
      FSM_servo3_FP = FSM_servo3_AP;
      FSM_GLOBAL_servo2_VAL = 0;
    break;
    case 21:
      FSM_GLOBAL_servo2_VAL = 1;
    break;
    case 22: //state for search alternative Z position downward
      FSM_servo2_FP = 5;
      FSM_GLOBAL_servo2_VAL = 0;
    break;
    case 23:
      FSM_GLOBAL_servo2_VAL = 1;
    break;
  }

}
// END FSM GLOBAL

///// FSM SERVO 0
void FSM_servo0(int FSM_servo0_FP, int FSM_GLOBAL_servo0_VAL){ 
  int ts_stimulus;
  if((millis() - ts_servo0) > servo_delay){ //Checks Delay
    ts_stimulus = 1;
  }else{
    ts_stimulus = 0;
  }

  
  FSM_servo0_nextstate((FSM_servo0_AP < FSM_servo0_FP),(FSM_servo0_AP > FSM_servo0_FP),(FSM_servo0_AP == FSM_servo0_FP),FSM_GLOBAL_servo0_VAL, ts_stimulus);
  FSM_servo0_output();
}

void FSM_servo0_nextstate(int m, int M, int I, int val, int ts_stimulus){
  switch(FSM_servo0_state){
    case 0:
    if(I && !val){ //IF AP == FP
      FSM_servo0_state = 3;
    } else if(m){ //IF AP is smaller
      FSM_servo0_state = 2;
    }else if(M){ //IF AP is bigger
      FSM_servo0_state = 1;
    }
    break;
    case 1:
      FSM_servo0_state = 4;
    break;
    case 2:
      FSM_servo0_state = 4;
    break;
    case 3:
    if (val == 1) FSM_servo0_state = 0;
    break;
    case 4:
    if(ts_stimulus){
      FSM_servo0_state = 0;
    }
    break;
  }
}

void FSM_servo0_output(){
  switch(FSM_servo0_state){
    case 0:
    FSM_servo0_VAL = 0;
    break;
    case 1:
      FSM_servo0_AP = FSM_servo0_AP - stepNumber;
      servo0.write(FSM_servo0_AP);
      ts_servo0 = millis();
    break;
    case 2:
      FSM_servo0_AP = FSM_servo0_AP + stepNumber;
      servo0.write(FSM_servo0_AP);
      ts_servo0 = millis();
    break;
    case 3:
      FSM_servo0_VAL = 1;
    break;
    case 4:
    break;
  }
}
// END FSM SERVO 0

///// FSM SERVO 1
void FSM_servo1(int FSM_servo1_FP, int FSM_GLOBAL_servo1_VAL){
  int ts_stimulus;
  if((millis() - ts_servo1) > servo_delay){ //Checks Delay
    ts_stimulus = 1;
  }else{
    ts_stimulus = 0;
  }
  
  FSM_servo1_nextstate((FSM_servo1_AP < FSM_servo1_FP),(FSM_servo1_AP > FSM_servo1_FP),(FSM_servo1_AP == FSM_servo1_FP), FSM_GLOBAL_servo1_VAL, ts_stimulus);
  FSM_servo1_output();
}

void FSM_servo1_nextstate(int m, int M, int I, int val, int ts_stimulus){
  switch(FSM_servo1_state){
    case 0:
    if(I && !val){ //IF AP == FP
      FSM_servo1_state = 3;
    } else if(m){ //IF AP is smaller
      FSM_servo1_state = 2;
    }else if(M){ //IF AP is bigger
      FSM_servo1_state = 1;
    }
    break;
    case 1:
      FSM_servo1_state = 4;
    break;
    case 2:
      FSM_servo1_state = 4;
    break;
    case 3:
    if (val == 1) FSM_servo1_state = 0;
    break;
    case 4:
    if(ts_stimulus){
      FSM_servo1_state = 0;
    }
    break;
  }
}

void FSM_servo1_output(){
  switch(FSM_servo1_state){
    case 0:
    FSM_servo1_VAL = 0;
    break;
    case 1:
      FSM_servo1_AP = FSM_servo1_AP - stepNumber;
      servo1.write(FSM_servo1_AP);
      ts_servo1 = millis();
    break;
    case 2:
      FSM_servo1_AP = FSM_servo1_AP + stepNumber;
      servo1.write(FSM_servo1_AP);
      ts_servo1 = millis();
    break;
    case 3:
      FSM_servo1_VAL = 1;
    break;
    case 4:
    break;
  }
}
// END FSM SERVO 1

///// FSM SERVO 2
void FSM_servo2(int FSM_servo2_FP, int FSM_GLOBAL_servo2_VAL){ 
  int ts_stimulus;
  if((millis() - ts_servo2) > servo_delay){ //Checks Delay
    ts_stimulus = 1;
  }else{
    ts_stimulus = 0;
  }

  
  FSM_servo2_nextstate((FSM_servo2_AP < FSM_servo2_FP),(FSM_servo2_AP > FSM_servo2_FP),(FSM_servo2_AP == FSM_servo2_FP), FSM_GLOBAL_servo2_VAL, ts_stimulus);
  FSM_servo2_output();
}

void FSM_servo2_nextstate(int m, int M, int I, int val, int ts_stimulus){
  switch(FSM_servo2_state){
    case 0:
    if(I && !val){ //IF AP == FP
      FSM_servo2_state = 3;
    } else if(m){ //IF AP is smaller
      FSM_servo2_state = 2;
    }else if(M){ //IF AP is bigger
      FSM_servo2_state = 1;
    }
    break;
    case 1:
      FSM_servo2_state = 4;
    break;
    case 2:
      FSM_servo2_state = 4;
    break;
    case 3:
    if (val == 1) FSM_servo2_state = 0;
    break;
    case 4:
    if(ts_stimulus){
      FSM_servo2_state = 0;
    }
    break;
  }
}

void FSM_servo2_output(){
  switch(FSM_servo2_state){
    case 0:
      FSM_servo2_VAL = 0;
    break;
    case 1:
      FSM_servo2_AP = FSM_servo2_AP - stepNumber;
      servo2.write(FSM_servo2_AP);
      ts_servo2 = millis();
    break;
    case 2:
      FSM_servo2_AP = FSM_servo2_AP + stepNumber;
      servo2.write(FSM_servo2_AP);
      ts_servo2 = millis();
    break;
    case 3:
      FSM_servo2_VAL = 1;
    break;
    case 4:
    break;
  }
}
// END FSM SERVO 2


///// FSM SERVO 3
void FSM_servo3(int FSM_servo3_FP, int FSM_GLOBAL_servo3_VAL){ 
  int ts_stimulus;
  if((millis() - ts_servo3) > servo_delay){ //Checks Delay
    ts_stimulus = 1;
  }else{
    ts_stimulus = 0;
  }

  
  FSM_servo3_nextstate((FSM_servo3_AP < FSM_servo3_FP),(FSM_servo3_AP > FSM_servo3_FP),(FSM_servo3_AP == FSM_servo3_FP), FSM_GLOBAL_servo3_VAL, ts_stimulus);
  FSM_servo3_output();
}

void FSM_servo3_nextstate(int m, int M, int I, int val, int ts_stimulus){
  switch(FSM_servo3_state){
    case 0:
    if(I && !val){ //IF AP == FP
      FSM_servo3_state = 3;
    } else if(m){ //IF AP is smaller
      FSM_servo3_state = 2;
    }else if(M){ //IF AP is bigger
      FSM_servo3_state = 1;
    }
    break;
    case 1:
      FSM_servo3_state = 4;
    break;
    case 2:
      FSM_servo3_state = 4;
    break;
    case 3:
    if (val == 1) FSM_servo3_state = 0;
    break;
    case 4:
    if(ts_stimulus){
      FSM_servo3_state = 0;
    }
    break;
  }
}

void FSM_servo3_output(){
  switch(FSM_servo3_state){
    case 0:
      FSM_servo3_VAL = 0;
    break;
    case 1:
      FSM_servo3_AP = FSM_servo3_AP - stepNumber;
      servo3.write(FSM_servo3_AP);
      ts_servo3 = millis();
    break;
    case 2:
      FSM_servo3_AP = FSM_servo3_AP + stepNumber;
      servo3.write(FSM_servo3_AP);
      ts_servo3 = millis();
    break;
    case 3:
      FSM_servo3_VAL = 1;
    break;
    case 4:
    break;
  }
}
// END FSM SERVO 3


///// FSM SERVO 4
void FSM_servo4(int FSM_servo4_FP, int FSM_GLOBAL_servo4_VAL){ 
  int ts_stimulus;
  if((millis() - ts_servo4) > servo_delay){ //Checks Delay
    ts_stimulus = 1;
  }else{
    ts_stimulus = 0;
  }

  
  FSM_servo4_nextstate((FSM_servo4_AP < FSM_servo4_FP),(FSM_servo4_AP > FSM_servo4_FP),(FSM_servo4_AP == FSM_servo4_FP),FSM_GLOBAL_servo4_VAL, ts_stimulus);
  FSM_servo4_output();
}

void FSM_servo4_nextstate(int m, int M, int I, int val, int ts_stimulus){
  switch(FSM_servo4_state){
    case 0:
    if(I && !val){ //IF AP == FP
      FSM_servo4_state = 3;
    } else if(m){ //IF AP is smaller
      FSM_servo4_state = 2;
    }else if(M){ //IF AP is bigger
      FSM_servo4_state = 1;
    }
    break;
    case 1:
      FSM_servo4_state = 4;
    break;
    case 2:
      FSM_servo4_state = 4;
    break;
    case 3:
    if (val == 1) FSM_servo4_state = 0;
    break;
    case 4:
    if(ts_stimulus){
      FSM_servo4_state = 0;
    }
    break;
  }
}

void FSM_servo4_output(){
  switch(FSM_servo4_state){
    case 0:
    FSM_servo4_VAL = 0;
    break;
    case 1:
      FSM_servo4_AP = FSM_servo4_AP - stepNumber;
      servo4.write(FSM_servo4_AP);
      ts_servo4 = millis();
    break;
    case 2:
      FSM_servo4_AP = FSM_servo4_AP + stepNumber;
      servo4.write(FSM_servo4_AP);
      ts_servo4 = millis();
    break;
    case 3:
      FSM_servo4_VAL = 1;
    break;
    case 4:
    break;
  }
}
// END FSM SERVO 4


///// FSM Obstacle
void FSM_Obstacle(int stimulus){
  
  FSM_Obstacle_nextstate(stimulus);
  FSM_Obstacle_output();
}

void FSM_Obstacle_nextstate(int stimulus){
  switch(FSM_Obstacle_state){
    case 0:
      if(stimulus){
        FSM_Obstacle_state = 1;
      }
    break;
    case 1:
      if(!stimulus){
        FSM_Obstacle_state = 0;
        }
    break;
  }
}

void FSM_Obstacle_output(){
  switch(FSM_Obstacle_state){
    case 0: //Activates interrupt
      digitalWrite(interruptPin, LOW);
    break;
    case 1: //Activates interrupt
      digitalWrite(interruptPin, HIGH);
    break;
  }
}
// END FSM Obstacle


  //Ultrasonic sensor without pulsein
  void getTime(){ //Interrupt Function connected with the echo pin of the Ultrasonic sensor
  Techo = micros();
  echoRead = 1;
}

//Ultrasonic FSM without pulseIn Function
void FSM_ultrasonic(byte echoRead, int reset_echoRead){ 
  
  FSM_ultrasonic_nextstate(echoRead);
  FSM_ultrasonic_output(reset_echoRead);
}

void FSM_ultrasonic_nextstate(byte echoRead){
  switch(FSM_ultrasonic_state){
    case 0:
      FSM_ultrasonic_state = 1;
    break;
    case 1:
      if(echoRead){
          FSM_ultrasonic_state = 2;
        }
      if((micros() - Ttrig) > 30000){ //Timeout
        FSM_ultrasonic_state = 0;
      }
      
    break;
    case 2:
      FSM_ultrasonic_state = 0;
    break;
  }
}

void FSM_ultrasonic_output(int reset_echoRead_local){ //////////////////No se est a utilizar o reset_echoRead_local/////////////////////////////////////
  switch(FSM_ultrasonic_state){
    case 0: //Sends trigger
      // Clears the trigPin
      digitalWrite(trigger_pin, LOW);
      delayMicroseconds(1);
      // Sets the trigPin on HIGH state for 10 micro seconds
      digitalWrite(trigger_pin, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigger_pin, LOW);

      Ttrig = micros();
    break;
    case 1:
      reset_echoRead = 1; ///////////////////////////////////////Porqu aqui?????////////////////////////////////////////////////
    break;
    case 2: //Receives echo
      if(echoRead){
        if(!duration){ //If duration is zero, avoids negative distance
          distance = 0;
        }else{
          distance = duration*0.034/2 - 7.28;
        }
        reset_echoRead = 1;
      }
    break;
   
    
  }
}
//END Ultrasonic FSM without pulseIn Function