Elevator with 4 floors and automatic doors

1#include <Ultrasonic.h>
2
3#define PINO_TRG  3
4#define PINO_ECHO  2
5
6float distancia_minima = 5;
7float distancia_maxima = 10;
8
9Ultrasonic  ultrasonic(PINO_TRG, PINO_ECHO);
10
11//INPUT FSM0
12#define FSM0_SFCF_pin 8
13bool  FSM0_SFCF;
14//INPUT FSM1
15#define FSM1_SFCA_pin 7
16bool FSM1_SFCA;
17
18//OUTPUT  FSM0
19int FSM0_SUBIR = A1;
20int FSM0_DESCER = A2;
21int FSM0_PARADO = A3;
22//OUTPUT  FSM1
23int FSM1_APORTA = A4;
24int FSM1_FPORTA = A5;
25
26static int FSM0_state  = 1; // initial state is 1, the "idle" state.
27static int FSM1_state = 1; //  initial state is 1, the "idle" state.
28static unsigned long FSM1_ts; // To store  the "current" time for delays.
29
30void setup(){
31  pinMode (FSM0_SFCF, INPUT);
32  pinMode (FSM1_SFCA, INPUT);
33  pinMode (FSM0_SUBIR, OUTPUT); pinMode (FSM0_DESCER,  OUTPUT); pinMode(FSM0_PARADO, OUTPUT);
34  pinMode (FSM1_APORTA, OUTPUT); pinMode  (FSM1_FPORTA, OUTPUT);
35  Serial.begin(9600);
36}
37
38void loop(){
39
40  float cmMsec;
41
42  long microsec = ultrasonic.timing();
43
44  cmMsec =  ultrasonic.convert(microsec, Ultrasonic::CM);
45
46  
47  int Pinloc0 = 12;
48  int Pinloc1 = 11;
49  int Pinloc2 = 10;
50  int Pinloc3 = 9;
51
52  int CA;
53  int SA;
54
55  bool E_MOV_DESC;
56  bool E_MOV_SUB;
57  bool E_MOV_PARA;
58
59  bool FSM1_E_time;
60  int TempoEspera=2000;
61
62  CA = Binary2Decimal(Pinloc0,  Pinloc1);
63  SA = Binary2Decimal(Pinloc2, Pinloc3);
64  Serial.print("CA: ");
65  Serial.println(CA);
66  Serial.print("SA: ");
67  Serial.println(SA);
68  delay(500); 
69
70  FSM0_SFCF = digitalRead(8);
71  FSM1_SFCA = digitalRead  (7);
72  
73  if(CA < SA && FSM0_SFCF == true){E_MOV_DESC = true;}else{E_MOV_DESC  = false;}
74  if(CA > SA && FSM0_SFCF == true){E_MOV_SUB = true;}else{E_MOV_SUB  = false;}
75  if(CA == SA && FSM0_SFCF == ture){E_MOV_PARA = true;}else{E_MOV_PARA  = false;}
76  if(millis() - FSM1_ts < TempoEspera){FSM1_E_time = true;}else{FSM1_E_time  = false;}
77  
78  bool PF;
79  if(FSM1_state == 1){PF = true;}
80  else{PF  = false;}
81    
82  FSM0(E_MOV_DESC, E_MOV_SUB, E_MOV_PARA, PF);
83  FSM1(FSM0_SFCF,  FSM1_SFCA, E_MOV_PARA, PF, cmMsec);
84}
85
86int Binary2Decimal(int x0, int x1)
87{
88  int Sum = 0;
89  
90  if (digitalRead(x0) == HIGH)
91    Sum += 1;
92  if  (digitalRead(x1) == HIGH)
93    Sum += 2;
94  return(Sum);
95}
96
97void FSM0(bool  E_MOV_DESC, bool E_MOV_SUB, bool E_MOV_PARA, bool PF) 
98{
99  FSM0_next_state(E_MOV_DESC,  E_MOV_SUB, E_MOV_PARA, PF); 
100  FSM0_output();
101}
102
103void FSM0_next_state(bool  E_MOV_DESC, bool E_MOV_SUB, bool E_MOV_PARA, bool PF) 
104{
105  switch(FSM0_state)
106  {
107    case 1:
108      if(E_MOV_SUB == true){FSM0_state = 2;}
109      if(E_MOV_DESC  == true){FSM0_state = 3;}
110      if(PF == false){FSM0_state = 1;}
111      break;
112    case 2:
113      if(E_MOV_PARA == true){FSM0_state = 1;}
114      break;
115    case 3:
116      if(E_MOV_PARA == true){FSM0_state = 1;}
117      break;
118  }
119
120    Serial.print(FSM0_state);
121    Serial.print("<-FSM0_state");
122    Serial.println();
123    delay(1000);
124}
125
126void FSM0_output()
127{
128  switch (FSM0_state)
129  {
130    case 1:
131      digitalWrite(A3, HIGH); //STOP
132      digitalWrite(A2, LOW); //DOWN
133      digitalWrite(A1, LOW); //UP
134      break;
135    case 2:
136      digitalWrite(A1, HIGH); //UP
137      digitalWrite(A3, LOW);  //STOP
138      digitalWrite(A2, LOW); //DOWN
139      digitalWrite(A4, LOW); //OPEN
140      digitalWrite(A5, LOW); //CLOSE
141      break;
142    case 3:
143      digitalWrite(A2,  HIGH); //DOWN
144      digitalWrite(A3, LOW); //STOP
145      digitalWrite(A1, LOW);  //UP
146      digitalWrite(A4, LOW); //OPEN
147      digitalWrite(A5, LOW); //CLOSE
148      break;
149  }
150}
151
152void FSM1(bool FSM0_SFCF,bool FSM1_SFCA,bool E_MOV_PARA,  bool FSM1_E_time, float cmMsec)
153{
154  FSM1_next_state(FSM0_SFCF, FSM1_SFCA,  E_MOV_PARA, FSM1_E_time, cmMsec);
155  FSM1_output();
156}
157
158void FSM1_next_state(bool  FSM0_SFCF, bool FSM1_SFCA, bool E_MOV_PARA, bool FSM1_E_time, float cmMsec)
159{
160  switch(FSM1_state)
161  {
162    case 1:
163      if(E_MOV_PARA == true){FSM1_state  = 2;}
164      break;
165    case 2:
166      if(FSM1_SFCA == true){FSM1_state =  3;}
167      break;
168    case 3:
169      if(FSM1_E_time == true){FSM1_state =  3;}else{FSM1_state = 4;}
170      break;
171    case 4:
172      if(FSM0_SFCF ==  true){FSM1_state = 5;}
173      if(cmMsec < distancia_minima){FSM1_state = 2;}
174      break;
175    case 5:
176      if(E_MOV_PARA == false){FSM1_state = 1;}
177      break;    
178  }
179    Serial.print(FSM1_state);
180    Serial.print("<-FSM1_state");
181    Serial.println();
182    delay(1000);
183}
184
185void FSM1_output()
186{
187  switch (FSM1_state)
188  {
189    case 1:
190      digitalWrite(A4, LOW); //OPEN
191      digitalWrite(A5, LOW); //CLOSE
192      break;
193    case 2:
194      digitalWrite(A4,  HIGH); //OPEN
195      digitalWrite(A5, LOW); //CLOSE
196      break;
197    case  3:
198      digitalWrite(A4, LOW); //OPEN
199      digitalWrite(A5, LOW); //CLOSE
200      break;
201    case 4:
202      digitalWrite(A4, LOW); //OPEN
203      digitalWrite(A5,  HIGH); //CLOSE
204      break;
205    case 5:
206      digitalWrite(A4, LOW); //OPEN
207      digitalWrite(A5, LOW); //CLOSE
208      break;
209  }
210}
211
212