Selsa

1
2#include <AFMotor.h>
3
4int trigPin = A2;
5int echoPin = A0;
6int
7  delaytime = 200;
8float duration;
9float distance;
10
11float soundVelocity
12  = 343;
13
14AF_DCMotor motor1(1, MOTOR12_64KHZ);
15AF_DCMotor motor2(2, MOTOR12_64KHZ);
16
17
18
19void
20  setup() {
21  AF_DCMotor begin();
22  pinMode(trigPin, OUTPUT);
23  pinMode(echoPin,
24  INPUT);
25}
26
27void forward()
28{
29  motor1.setSpeed(255);
30  motor2.setSpeed(255);
31
32  motor1.run(FORWARD);
33  motor2.run(FORWARD);
34}
35
36void backward()
37{
38
39  motor1.setSpeed(255);
40  motor1.run(BACKWARD);
41  motor2.setSpeed(255);
42
43  motor2.run(BACKWARD);
44}
45
46void move_stop()
47{
48  motor1.run(RELEASE);
49
50  motor2.run(RELEASE);
51}
52
53void turn()
54{
55  motor1.setSpeed(255);
56
57  motor1.run(BACKWARD);
58  motor2.setSpeed(255);
59  motor2.run(FORWARD);
60
61
62  Serial.begin(9600);
63}
64
65void loop() {
66
67  
68
69  digitalWrite(trigPin,
70  LOW);
71  delayMicroseconds(10);
72
73  digitalWrite(trigPin, HIGH);
74  delay(delaytime);
75
76  digitalWrite(trigPin, LOW);
77
78  duration = pulseIn(echoPin, HIGH);
79  duration
80  = duration / 1e+6; // to change the second from microsecond
81
82  duration =
83  duration/2; //to get the duration for one time
84
85  distance = soundVelocity
86  * duration;
87
88  float final_distance = distance * 10; // to get the centimeter
89
90
91  Serial.println(final_distance);
92
93
94  
95  if (final_distance < 5)
96
97  {
98    move_stop();
99    delay(10);
100    backward();
101    delay(500);
102
103    turn();
104    delay(1000);
105    final_distance = 0;
106  }
107
108  else
109
110  {
111    forward();
112    delay(90);
113  }
114}
115

1
2#include <AFMotor.h>
3
4int trigPin = A2;
5int echoPin = A0;
6int  delaytime = 200;
7float duration;
8float distance;
9
10float soundVelocity  = 343;
11
12AF_DCMotor motor1(1, MOTOR12_64KHZ);
13AF_DCMotor motor2(2, MOTOR12_64KHZ);
14
15
16
17void  setup() {
18  AF_DCMotor begin();
19  pinMode(trigPin, OUTPUT);
20  pinMode(echoPin,  INPUT);
21}
22
23void forward()
24{
25  motor1.setSpeed(255);
26  motor2.setSpeed(255);
27  motor1.run(FORWARD);
28  motor2.run(FORWARD);
29}
30
31void backward()
32{
33  motor1.setSpeed(255);
34  motor1.run(BACKWARD);
35  motor2.setSpeed(255);
36  motor2.run(BACKWARD);
37}
38
39void move_stop()
40{
41  motor1.run(RELEASE);
42  motor2.run(RELEASE);
43}
44
45void turn()
46{
47  motor1.setSpeed(255);
48  motor1.run(BACKWARD);
49  motor2.setSpeed(255);
50  motor2.run(FORWARD);
51
52  Serial.begin(9600);
53}
54
55void loop() {
56
57  
58
59  digitalWrite(trigPin,  LOW);
60  delayMicroseconds(10);
61
62  digitalWrite(trigPin, HIGH);
63  delay(delaytime);
64  digitalWrite(trigPin, LOW);
65
66  duration = pulseIn(echoPin, HIGH);
67  duration  = duration / 1e+6; // to change the second from microsecond
68
69  duration =  duration/2; //to get the duration for one time
70
71  distance = soundVelocity  * duration;
72
73  float final_distance = distance * 10; // to get the centimeter
74
75  Serial.println(final_distance);
76
77
78  
79  if (final_distance < 5)
80  {
81    move_stop();
82    delay(10);
83    backward();
84    delay(500);
85    turn();
86    delay(1000);
87    final_distance = 0;
88  }
89
90  else
91  {
92    forward();
93    delay(90);
94  }
95}
96