Scribd
Upload
15 views

Project - Code

The document defines two finite state machines (FSM0 and FSM1) to control the movement and opening/closing of a system. FSM0 controls the movement and has 3 states - up, down, and stopped. It transitions between states based on sensor readings. FSM1 controls the opening/closing and has 5 states. It transitions between states based on sensor readings and timing. Both FSMs output signals to control motors and actuators based on their current state.

Uploaded by

samisamata7
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
15 views

Project - Code

The document defines two finite state machines (FSM0 and FSM1) to control the movement and opening/closing of a system. FSM0 controls the movement and has 3 states - up, down, and stopped. It transitions between states based on sensor readings. FSM1 controls the opening/closing and has 5 states. It transitions between states based on sensor readings and timing. Both FSMs output signals to control motors and actuators based on their current state.

Uploaded by

samisamata7
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 4

#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;
}
}

You might also like