1#include <Servo.h>
2Servo Lo;
3Servo Lt;
4Servo Ro;
5Servo Rt;
6Servo Bo;
7Servo Bt;
8Servo Mo;
9Servo Mt;
10Servo Eo;
11Servo Et;
12
13
14int delay_val = 500;
15int angle = 120;
16int go = 0;
17char detect = "on";
18
19const int trigPin = 13;
20const int echoPin = A2;
21long duration;
22int distance;
23
24void setup() {
25 Serial.begin(9600);
26 pinMode(trigPin, OUTPUT);
27 pinMode(echoPin, INPUT);
28 Lo.attach(7);
29 Lt.attach(10);
30 Eo.attach(12);
31 Et.attach(2);
32 Mo.attach(4);
33 Mt.attach(3);
34 Bo.attach(9);
35 Bt.attach(A5);
36 Ro.attach(5);
37 Rt.attach(6);
38
39 Bo.write(180);
40 Bt.write(180);
41 Lo.write(0);
42 Lt.write(0);
43 Ro.write(0);
44 Rt.write(0);
45 Mo.write(0);
46 Mt.write(0);
47 Eo.write(0);
48 Et.write(10);
49}
50
51void Routine(){
52 if (go > 0) {
53 delay(delay_val);
54 Bt.write(180 - angle);
55 Bo.write(180 - angle);
56 delay(delay_val);
57 Bt.write(180);
58 Bo.write(180);
59 Rt.write(angle);
60 Ro.write(angle);
61 Lt.write(angle);
62 Lo.write(angle);
63 delay(delay_val);
64 Rt.write(0);
65 Ro.write(0);
66 Lt.write(0);
67 Lo.write(0);
68
69 delay(delay_val);
70 Mt.write(260);
71 Mo.write(260);
72 delay(870);
73 Mt.write(0);
74 Mo.write(0);
75
76 delay(400);
77 Mt.write(0);
78 Mo.write(0);
79 Et.write(270);
80 Eo.write(270);
81 delay(800);
82 Et.write(10);
83 Eo.write(0);
84 delay(3000);
85 }
86}
87
88void loop() {
89 Serial.println(distance);
90 digitalWrite(trigPin, LOW);
91 delayMicroseconds(2);
92
93 digitalWrite(trigPin, HIGH);
94 delayMicroseconds(10);
95 digitalWrite(trigPin, LOW);
96
97 duration = pulseIn(echoPin, HIGH);
98
99 distance = duration * 0.034 / 2;
100
101 if (distance > 300) {
102 go = 1;
103 Routine();
104 go = 0;
105 }
106}