Codigo de semaforo para arduino

Semaforo.ino

#define peatonInPin    12
#define peatonVerde     3
#define peatonRojo      2

#define sem1Verde       5
#define sem1Rojo        6
#define sem1Amarillo    7
#define sem2Amarillo A3
#define sem2Rojo    A2
#define sem2Verde   A4


bool hayPeaton = false;
long previousMillis = 0; 
long intervalo = 500; 
int step = 1;
void setup() {
  pinMode(peatonInPin,INPUT);
  pinMode(peatonVerde,OUTPUT);
  pinMode(peatonRojo,OUTPUT);
  pinMode(sem1Amarillo,OUTPUT);
  pinMode(sem2Amarillo,OUTPUT);
  pinMode(sem1Rojo,OUTPUT);
  pinMode(sem2Rojo,OUTPUT);
  pinMode(sem1Verde,OUTPUT);
  pinMode(sem2Verde,OUTPUT);
  //Serial.begin(9600);
  //test();
}

void loop() {
    long currentMillis = millis();
    
    if(digitalRead(peatonInPin)==HIGH){
        hayPeaton = true;
    }
    if (currentMillis - previousMillis >= intervalo) {
        previousMillis = currentMillis;
        if(hayPeaton && (step == 0 || step ==4)){
            Serial.println(1);
            pasarPeaton();
            hayPeaton = false;
            //analogWrite(A1,150);
            intervalo = 1500;
            tone(A1,262,intervalo);
        }else{
            Serial.println(0);
            detenerPeaton();
            analogWrite(A1,0);
            sem();
            intervalo = 500;
        }
    }
}

void pasarPeaton(){
    apagarTodo();
    digitalWrite(peatonVerde,HIGH);
    digitalWrite(sem1Rojo,HIGH);
    digitalWrite(sem2Rojo,HIGH);
    step=1;
    
}
void detenerPeaton(){
    digitalWrite(peatonRojo,HIGH);
    digitalWrite(peatonVerde,LOW);
}
void apagarTodo(){
    digitalWrite(peatonVerde,LOW);
    digitalWrite(peatonRojo,LOW);
    digitalWrite(sem1Amarillo,LOW);
    digitalWrite(sem2Amarillo,LOW);
    digitalWrite(sem1Verde,LOW);
    digitalWrite(sem1Rojo,LOW);
    digitalWrite(sem2Verde,LOW);
    digitalWrite(sem2Rojo,LOW);
}

void sem(){
    apagarTodo();
    digitalWrite(peatonRojo,HIGH);
    switch(step){
        case 0:
            digitalWrite(sem2Rojo,HIGH);
            digitalWrite(sem1Rojo,HIGH);
            step++;
        break;
        case 1:
            digitalWrite(sem2Rojo,HIGH);
            digitalWrite(sem1Amarillo,HIGH);
            step++;
        break;
        case 2:
            digitalWrite(sem2Rojo,HIGH);
            digitalWrite(sem1Verde,HIGH);
            step++;
        break;
        case 3:
            digitalWrite(sem2Rojo,HIGH);
            digitalWrite(sem1Amarillo,HIGH);
            step++;
        break;
        case 4:
            digitalWrite(sem2Rojo,HIGH);
            digitalWrite(sem1Rojo,HIGH);
            step++;
        break;
        case 5:
            digitalWrite(sem2Amarillo,HIGH);
            digitalWrite(sem1Rojo,HIGH);
            step++;
        break;
        case 6:
            digitalWrite(sem2Verde,HIGH);
            digitalWrite(sem1Rojo,HIGH);
            step++;
        break;
        case 7:
            digitalWrite(sem2Amarillo,HIGH);
            digitalWrite(sem1Rojo,HIGH);
            step=0;
        break;

    }
}

void test(){
    int time = 500;
    digitalWrite(peatonVerde,HIGH);
    delay(time);
    digitalWrite(peatonRojo,HIGH);
    delay(time);
    digitalWrite(sem1Verde,HIGH);
    delay(time);
    digitalWrite(sem1Amarillo,HIGH);
    delay(time);
    digitalWrite(sem1Rojo,HIGH);
    delay(time);
    digitalWrite(sem2Verde,HIGH);
    delay(time);
    digitalWrite(sem2Amarillo,HIGH);
    delay(time);
    digitalWrite(sem2Rojo,HIGH);    
    delay(time*10);
    apagarTodo();
}