Motion Direction Detector

1#define trigPin1 2
2#define echoPin1 3
3#define trigPin2 4
4#define  echoPin2 5
5
6
7int Led3=10;
8int Led1=11;
9int Led2=12;
10int sensor1=0;
11int  sensor2=0;
12float duration1,distance1, distance2, duration2;
13volatile int FSM1_estado=0;
14int  contagem=0;
15unsigned long tempo =0;
16unsigned long r,s,x;
17
18
19
20void  setup() {
21Serial.begin (9600);
22pinMode(trigPin1, OUTPUT);
23pinMode(echoPin1,  INPUT);
24pinMode(Led1, OUTPUT);
25pinMode(trigPin2, OUTPUT);
26pinMode(echoPin2,  INPUT);
27pinMode(Led2, OUTPUT);
28pinMode(Led3, OUTPUT);
29}
30
31
32void  loop() {
33
34  digitalWrite(trigPin1, LOW);
35  delayMicroseconds(2);
36  digitalWrite(trigPin1,  HIGH);
37  delayMicroseconds(5);
38  digitalWrite(trigPin1, LOW);
39
40  duration1=pulseIn  (echoPin1, HIGH); //tini=micros();
41  distance1= (duration1/2)*0.0345;
42
43   digitalWrite(Led1, LOW);
44   digitalWrite(Led2, LOW);
45   digitalWrite(Led3,  LOW);
46   if(distance1 >=5 && distance1 <=15){
47    sensor1=1;
48  
49     if  (sensor1==1){
50        
51      FSM1();
52      }
53   }
54   else{sensor1=0;}
55  
56}
57
58
59
60
61void FSM1(){
62int contador;
63int s=1000;
64
65
66switch(FSM1_estado){
67
68  case 0:
69   digitalWrite(Led1, LOW);
70   digitalWrite(Led2, LOW);
71   digitalWrite(Led3,  LOW);
72  
73   FSM1_estado=1;
74   break;
75 
76  
77  
78  
79  case 1:
80   r=millis();
81   digitalWrite(Led1, HIGH);
82   digitalWrite(Led2, LOW);
83   digitalWrite(Led3, LOW);
84   FSM1_estado=2;
85  break;
86
87
88 case 2:
89      
90       delay(60);
91       digitalWrite(trigPin2, HIGH);
92       delayMicroseconds(500);
93       digitalWrite(trigPin2, LOW);
94       duration2=pulseIn (echoPin2, HIGH);
95       distance2= (duration2/2)*0.0344;
96        x=millis ()-r;
97        
98         if ((distance2>=5 && distance2 <15)&&(x<s)){      
99                   
100            FSM1_estado=3; 
101       }       
102            else{FSM1_estado=4;}     
103   break;
104
105
106 case 3:
107   digitalWrite(Led1, HIGH);
108   digitalWrite(Led2,  HIGH);
109   digitalWrite(Led3, LOW);    
110  Serial.println();
111  Serial.print("-------------------------------------------");
112  Serial.println();
113  Serial.print("-----------SEQUENCIA CORRETA--------------  ");
114  Serial.println();
115  Serial.print("-------------------------------------------");
116  Serial.println();
117  contagem++;
118  Serial.println(contagem);
119   FSM1_estado=0;
120  break;
121
122
123 case 4:
124  digitalWrite(Led1, LOW);
125  digitalWrite(Led2,  LOW);
126  digitalWrite(Led3, HIGH);    
127  Serial.println();
128  Serial.print("-------------------------------------------");
129  Serial.println();
130  Serial.print("-----------INCORRETA ALRME --------------  ");
131  Serial.println();
132  Serial.print("-------------------------------------------");
133  Serial.println();
134  delay (200);
135   FSM1_estado=0;
136  break;
137 
138
139
140}
141
142}
143
144
145
146
147  
148
149
150
151
152