#include <Ultrasonic.

h>

#define PINO_TRG 3
#define PINO_ECHO 2

float distancia_minima = 5;
float distancia_maxima = 10;

Ultrasonic ultrasonic(PINO_TRG, PINO_ECHO);

//INPUT FSM0
#define FSM0_SFCF_pin 8
bool FSM0_SFCF;
//INPUT FSM1
#define FSM1_SFCA_pin 7
bool FSM1_SFCA;

//OUTPUT FSM0
int FSM0_SUBIR = A1;
int FSM0_DESCER = A2;
int FSM0_PARADO = A3;
//OUTPUT FSM1
int FSM1_APORTA = A4;
int FSM1_FPORTA = A5;

static int FSM0_state = 1; // initial state is 1, the "idle" state.

static int FSM1_state = 1; // initial state is 1, the "idle" state.
static unsigned long FSM1_ts; // To store the "current" time for delays.

void setup(){
pinMode (FSM0_SFCF, INPUT);
pinMode (FSM1_SFCA, INPUT);
pinMode (FSM0_SUBIR, OUTPUT); pinMode (FSM0_DESCER, OUTPUT);
pinMode(FSM0_PARADO, OUTPUT);
pinMode (FSM1_APORTA, OUTPUT); pinMode (FSM1_FPORTA, OUTPUT);
Serial.begin(9600);
}

void loop(){

float cmMsec;

long microsec = ultrasonic.timing();

cmMsec = ultrasonic.convert(microsec, Ultrasonic::CM);

int Pinloc0 = 12;

int Pinloc1 = 11;
int Pinloc2 = 10;
int Pinloc3 = 9;

int CA;
int SA;

bool E_MOV_DESC;
bool E_MOV_SUB;
bool E_MOV_PARA;
 bool FSM1_E_time;
int TempoEspera=2000;

CA = Binary2Decimal(Pinloc0, Pinloc1);
SA = Binary2Decimal(Pinloc2, Pinloc3);
Serial.print("CA: ");
Serial.println(CA);
Serial.print("SA: ");
Serial.println(SA);
delay(500);

FSM0_SFCF = digitalRead(8);
FSM1_SFCA = digitalRead (7);

if(CA < SA && FSM0_SFCF == true){E_MOV_DESC = true;}else{E_MOV_DESC = false;}

if(CA > SA && FSM0_SFCF == true){E_MOV_SUB = true;}else{E_MOV_SUB = false;}
if(CA == SA && FSM0_SFCF == ture){E_MOV_PARA = true;}else{E_MOV_PARA = false;}
if(millis() - FSM1_ts < TempoEspera){FSM1_E_time = true;}else{FSM1_E_time =
false;}

bool PF;
if(FSM1_state == 1){PF = true;}
else{PF = false;}

FSM0(E_MOV_DESC, E_MOV_SUB, E_MOV_PARA, PF);

FSM1(FSM0_SFCF, FSM1_SFCA, E_MOV_PARA, PF, cmMsec);
}

int Binary2Decimal(int x0, int x1)

{
int Sum = 0;

if (digitalRead(x0) == HIGH)
Sum += 1;
if (digitalRead(x1) == HIGH)
Sum += 2;
return(Sum);
}

void FSM0(bool E_MOV_DESC, bool E_MOV_SUB, bool E_MOV_PARA, bool PF)

{
FSM0_next_state(E_MOV_DESC, E_MOV_SUB, E_MOV_PARA, PF);
FSM0_output();
}

void FSM0_next_state(bool E_MOV_DESC, bool E_MOV_SUB, bool E_MOV_PARA, bool PF)

{
switch(FSM0_state)
{
case 1:
if(E_MOV_SUB == true){FSM0_state = 2;}
if(E_MOV_DESC == true){FSM0_state = 3;}
if(PF == false){FSM0_state = 1;}
break;
case 2:
if(E_MOV_PARA == true){FSM0_state = 1;}
break;
case 3:
if(E_MOV_PARA == true){FSM0_state = 1;}
 break;
}

Serial.print(FSM0_state);
Serial.print("<-FSM0_state");
Serial.println();
delay(1000);
}

void FSM0_output()
{
switch (FSM0_state)
{
case 1:
digitalWrite(A3, HIGH); //STOP
digitalWrite(A2, LOW); //DOWN
digitalWrite(A1, LOW); //UP
break;
case 2:
digitalWrite(A1, HIGH); //UP
digitalWrite(A3, LOW); //STOP
digitalWrite(A2, LOW); //DOWN
digitalWrite(A4, LOW); //OPEN
digitalWrite(A5, LOW); //CLOSE
break;
case 3:
digitalWrite(A2, HIGH); //DOWN
digitalWrite(A3, LOW); //STOP
digitalWrite(A1, LOW); //UP
digitalWrite(A4, LOW); //OPEN
digitalWrite(A5, LOW); //CLOSE
break;
}
}

void FSM1(bool FSM0_SFCF,bool FSM1_SFCA,bool E_MOV_PARA, bool FSM1_E_time, float

cmMsec)
{
FSM1_next_state(FSM0_SFCF, FSM1_SFCA, E_MOV_PARA, FSM1_E_time, cmMsec);
FSM1_output();
}

void FSM1_next_state(bool FSM0_SFCF, bool FSM1_SFCA, bool E_MOV_PARA, bool

FSM1_E_time, float cmMsec)
{
switch(FSM1_state)
{
case 1:
if(E_MOV_PARA == true){FSM1_state = 2;}
break;
case 2:
if(FSM1_SFCA == true){FSM1_state = 3;}
break;
case 3:
if(FSM1_E_time == true){FSM1_state = 3;}else{FSM1_state = 4;}
break;
case 4:
if(FSM0_SFCF == true){FSM1_state = 5;}
if(cmMsec < distancia_minima){FSM1_state = 2;}
 break;
case 5:
if(E_MOV_PARA == false){FSM1_state = 1;}
break;
}
Serial.print(FSM1_state);
Serial.print("<-FSM1_state");
Serial.println();
delay(1000);
}

void FSM1_output()
{
switch (FSM1_state)
{
case 1:
digitalWrite(A4, LOW); //OPEN
digitalWrite(A5, LOW); //CLOSE
break;
case 2:
digitalWrite(A4, HIGH); //OPEN
digitalWrite(A5, LOW); //CLOSE
break;
case 3:
digitalWrite(A4, LOW); //OPEN
digitalWrite(A5, LOW); //CLOSE
break;
case 4:
digitalWrite(A4, LOW); //OPEN
digitalWrite(A5, HIGH); //CLOSE
break;
case 5:
digitalWrite(A4, LOW); //OPEN
digitalWrite(A5, LOW); //CLOSE
break;
}
}