Traffic Light with Treadmill and Sensor

int carro_vermelho_pin = 12;   //semaforo vermelho
int carro_amarelo_pin = 11;    //semaforo amanrelo
int carro_verde_pin = 10;       // semaforo verde
int passadeira_vermelho_pin = 8;
int passadeira_verde_pin = 7;
int botao = 9;            // pin do botao
int cross_time = 0;   //tempo para os peoes atravessarem
int tempo_1s = 0;
const int pin_echo = 2;
const int pin_trig = 5;
volatile int techo;
int estimulo_ss;
int  duracao2;
unsigned long ttrig;
volatile int leitura;
int cm2;
long duration;
int tempo, cm;

static int maquina_semaforo_state = 1; // estado inicial da maquina do semaforo
static int maquina_sensor_state = 1; // estado inicial da maquina do sensor

void setup() {
  attachInterrupt(0, pin_ISR, CHANGE);
  pinMode(pin_trig, OUTPUT);
  pinMode(pin_echo, INPUT);
  pinMode(carro_vermelho_pin, OUTPUT);
  pinMode(carro_amarelo_pin, OUTPUT);
  pinMode(carro_verde_pin, OUTPUT);
  pinMode(passadeira_vermelho_pin, OUTPUT);
  pinMode(passadeira_verde_pin, OUTPUT);
  pinMode(botao, INPUT);    // botao
  /// desliga todos os LEDS ///
  digitalWrite(carro_vermelho_pin, LOW);
  digitalWrite(carro_amarelo_pin, LOW);
  digitalWrite(carro_verde_pin, LOW);
  digitalWrite(passadeira_vermelho_pin, LOW);
  digitalWrite(passadeira_verde_pin, LOW);
  Serial.begin(9600);

}

void loop() {
  int cross_time_time = 10000;
  int distancia_seguranca = 20;
  int distancia_ninguem = 2;
  int time_delay = 2000;
  int estimulo_1;
  int estimulo_cross;
  int state_botao = digitalRead(botao);       // le o valor 0 ou 1 do botao
  estimulo_ss = 1;


        Serial.print(cm2 - 7); //calibration constant 7cm
        Serial.print("cm2");
        Serial.println();

  if (millis() - cross_time > cross_time_time && cross_time != 0 && ((cm2-7) > distancia_seguranca ||(cm2-7) < distancia_ninguem)) {  // verifica se o tempo atual - o cross time foi maior que 10seg
    estimulo_1 = 0; estimulo_cross = 1;          // verifica que nao esta nunquem na passadeira e que o tempo passou
    Serial.println("Cross");
//    while(1);
  } else {
    if (millis() - tempo_1s > time_delay && tempo_1s != 0) {    // se o tempo do dalay passou
      estimulo_1 = 1; estimulo_cross = 0;
      // maquina_semaforo(estimulo_1, estimulo_cross, state_botao);
      Serial.println("1s");
    } else {
      estimulo_1 = 0; estimulo_cross = 0;   // reinicio os estimos do delay e da passadeira
      // maquina_semaforo(estimulo_1, estimulo_cross, state_botao);
      Serial.println("nada");
    }

  }

  maquina_semaforo(estimulo_1, estimulo_cross, state_botao);        // passa prara o estado a seguir
  maquina_sensor();
}







void maquina_sensor()
{
  maquina_sensor_next_state(estimulo_ss);
  maquina_sensor_outputs();
}


void maquina_sensor_next_state(int estimulo_ss)
{
  switch (maquina_sensor_state)
  {
    case 1:
      {
        if (estimulo_ss = 1)
        {
          maquina_sensor_state = 2;
        }
        break;
      }
    case 2:
      {
        maquina_sensor_state = 3;
        break;
      }
    case 3:
      {
        if (leitura == 1) {
          maquina_sensor_state = 1;
        }
        break;
      }
  }
}

void maquina_sensor_outputs()
{
  switch (maquina_sensor_state)
  {
    case 1:
      {
        leitura = 0;
        break;
      }
    case 2:
      {
        digitalWrite(pin_trig, LOW);
        delayMicroseconds(2);
        digitalWrite(pin_trig, HIGH);
        delayMicroseconds(5);
        digitalWrite(pin_trig, LOW);
         ttrig = micros();
        //leitura = digitalRead(pin_echo);
        break;
      }
    case 3:
      {
        Serial.print(leitura);
        Serial.print("leitura");
        Serial.println();
        duracao2 = techo - ttrig;
        cm2 = microsecondsToCentimeters(duracao2);
//        Serial.print(cm2 - 7); //calibration constant 7cm
//        Serial.print("cm2");
//        Serial.println();
        break;
      }
  }
}

void maquina_semaforo_outputs(int state_botao)                                  // outputs                     int do botao 
{
  
  switch(maquina_semaforo_state)
  {
    case 1:
    {
      if(state_botao==HIGH)
      {
        
        digitalWrite(12, LOW);   //semafero Verme
        digitalWrite(carro_amarelo_pin, LOW);
        digitalWrite(carro_verde_pin, HIGH);
        digitalWrite(passadeira_vermelho_pin, HIGH);     //pass vermelho
        digitalWrite(passadeira_verde_pin, LOW);
      }
      tempo_1s = 0;
      break;
    }

    case 2:
    {
      digitalWrite(12, LOW);
      digitalWrite(carro_amarelo_pin, HIGH);
      digitalWrite(carro_verde_pin, LOW);
      tempo_1s = millis();                             // registar o tempo atual 
      break;
    }

    case 3: //delay
    {
      break;
    }
    
    
    case 4:
    {
      digitalWrite(12, HIGH);
      digitalWrite(carro_amarelo_pin, LOW);
      digitalWrite(carro_verde_pin, LOW);
      tempo_1s = millis();
      break;
    }

    case 5:
    {
      //delay
      break;
    }

    case 6:
    {
      digitalWrite(passadeira_verde_pin, HIGH);
      digitalWrite(passadeira_vermelho_pin, LOW);
      cross_time = millis();
      tempo_1s = 0;
      break;
    }

    case 7:
    {
      //delay cross time
      break;
    }

    case 8:
    {
      digitalWrite(passadeira_verde_pin, LOW);
      digitalWrite(passadeira_vermelho_pin, HIGH);
      tempo_1s = millis();
      //cross_time = 0;
      break;
    }

    case 9:
    {
      //delay
      break;
    }
//    Serial.print(maquina_semaforo_state);
//    Serial.print("<-maquina_semaforo_state");
//    Serial.println();
  }
}

void maquina_semaforo(int estimulo_1, int estimulo_cross, int state_botao){

           // verifica se o tempo atual - o cross time foi maior que 10seg
      maquina_semaforo_next_state(estimulo_1, estimulo_cross, state_botao);        // passa prara o estado a seguir 
      maquina_semaforo_outputs(state_botao);              


}

void maquina_semaforo_next_state(int estimulo_1, int estimulo_cross, int state_botao){                                  // estimulo e o valor do tempo se passou ou nao a 0 ou 1
  
  switch(maquina_semaforo_state)                                                            // maquina de delays so faz os delays mais nada
  {
    case 1:
    {
      if(state_botao==LOW)
        {
         maquina_semaforo_state=2;
        }
      break;
    }

    case 2:
    {
      maquina_semaforo_state=3;
      break;
    }

    case 3:
    {
      if(estimulo_1 == 1){                   // vindo de cima
        maquina_semaforo_state=4;
      }
      break;
    }
    
    
    case 4:
    {
      maquina_semaforo_state=5;
      break;
    }

    case 5:
    {
      if(estimulo_1 == 1){
        maquina_semaforo_state=6;
      }
      break;
    }

    case 6:
    {
      maquina_semaforo_state=7;
      break;
    }

    case 7:
    {
      if(estimulo_cross == 1 ){
        maquina_semaforo_state=8;
      }
      break;
    }

    case 8:
    {
      maquina_semaforo_state=9;
      break;
    }

    case 9:
    {
      if(estimulo_1 == 1){
        maquina_semaforo_state=1;
      }
      maquina_semaforo_state=1;
      break; 
    }
//    Serial.print(maquina_semaforo_state);
//    Serial.print("<-maquina_semaforo_state");
//    Serial.println();
  }
  Serial.println(maquina_semaforo_state);
}


void pin_ISR() {
  techo = micros();
  leitura = 1;
}

long microsecondsToCentimeters(long microseconds) {
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the object we
  // take half of the distance travelled.
  return microseconds / 29 / 2;
}