RoboCar/Obstacle.ino at main · aswanthabam/RoboCar · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
#include<Servo.h>

#define ul_echo 2 // Ultrasonic sensor Echo pin
#define ul_trig 3  // Ultrasonic Sensor Trig pin

#define ML_1 4  // left side motors control input 1
#define ML_2 5  // left side motors control input 2
#define MR_1 6  // right side motors control input 1
#define MR_2 7  // Roght side motors control input 2
#define M_speed 9 // All motor speed pin (ENA & ENB pins)
# define M_delay90 500 // Delay needed for rotating the car 90 degree

#define S_pin 10 // Servo motor pin
#define spoint 103 // Servo default position

Servo servo;
int distance,L,R,Left,Right;

void setup(){
  pinMode(ul_echo,OUTPUT);
  pinMode(ul_trig,INPUT);
  servo.attach(S_pin);
}

void loop(){
  obstacle();
}

void left(){
  digitalWrite(MR_1,HIGH);
  digitalWrite(MR_2,LOW);
  digitalWrite(ML_1,LOW);
  digitalWrite(ML_2,HIGH);
}

void right(){
  digitalWrite(MR_1,LOW);
  digitalWrite(MR_2,HIGH);
  digitalWrite(ML_1,HIGH);
  digitalWrite(ML_2,LOW);
}
void forward(){
  digitalWrite(MR_1,HIGH);
  digitalWrite(MR_2,LOW);
  digitalWrite(ML_1,HIGH);
  digitalWrite(ML_2,LOW);
}
void backward(){
  digitalWrite(MR_1,LOW);
  digitalWrite(MR_2,HIGH);
  digitalWrite(ML_1,LOW);
  digitalWrite(ML_2,HIGH);
}
void stop(){
  backward();
  delay(100);
  analogWrite(M_speed,0);
}
void obstacle (){
  distance = ultrasonic(); // Get the current distance
  if (distance <= 12) {
    // Distance less than 12 Cm
    stop();
    /*backward();
    delay(100);
    stop();*/
    L = leftsee();
    servo.write(spoint);
    delay(800);
    R = rightsee();
    servo.write(spoint);
    if (L < R) {
      right();
      delay(M_delay90);
      stop();
      delay(200);
    } else if (L > R) {
      left();
      delay(M_delay90);
      stop();
      delay(200);
    }
  } else {
    forward();
  }
}

int ultrasonic() {
  // Read the value of ul sensor and return it.in cm
  digitalWrite(ul_trig, LOW);
  delayMicroseconds(4);
  digitalWrite(ul_trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(ul_trig, LOW);
  long t = pulseIn(ul_echo, HIGH);
  long cm = t / 29 / 2; //time convert distance
  return cm;
}
int rightsee() {
  // Rotate the servo motor to right side and return the ultrasonic sensor value
  servo.write(20);
  delay(800);
  Left = ultrasonic();
  return left;
}

int leftsee() {
  // Rotate the servo to left side and return the ultrasonic sensor value
  servo.write(180);
  delay(800);
  Right = ultrasonic();
  return right;
}