Робот с УЗ датчиком

 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

/* Экспериментируем с ультразвуковым датчиком HC-SR04.
Если расстояние до препятствия меньше 50 см, то включаем светодиод на плате ардуино
 */

int Trig = 8; //обозначим к какой ножке и что подключаем соответствующий пин датчика 
int Echo = 9; 
int ledPin = 13;  // Светодиод на плате Ардуино

void setup() 
{ 
pinMode(Trig, OUTPUT); 
pinMode(Echo, INPUT); 
pinMode(ledPin, OUTPUT); 
  
Serial.begin(9600); //Инициализируем сериал порт для вывода полученных данных
} 
unsigned int time_us=0; //Переменная для хранения временного интервала 
unsigned int distance_sm=0; // Переменная для хранения расстояния в сантиметрах

void loop() 
{ 
digitalWrite(Trig, HIGH); // Подаем сигнал на выход микроконтроллера 
delayMicroseconds(10); // Удерживаем 10 микросекунд 
digitalWrite(Trig, LOW); // выключаем импульс
time_us=pulseIn(Echo, HIGH); // Замеряем длину принятого импульса 
distance_sm=time_us/58; // Пересчитываем в сантиметры 

Serial.println(distance_sm); // Выводим в порт
if (distance_sm<50) // Если расстояние менее 50 сантиметром 
{ 
   digitalWrite(ledPin, HIGH); // Включаем светодиод 
} 
else 
{ 
   digitalWrite(ledPin, LOW); // Иначе выключаем 
} 
  
delay(100); 
}

  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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146

#include 
#include  

//#define TRIG_PIN A4 
//#define ECHO_PIN A5 
//#define MAX_DISTANCE 200 
//NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); 
NewPing sonar(12, 11, 200);
Servo myservo;   

boolean goesForward=false;
int distance = 100;
int speedSet = 0;

//левый мотор
const int motorIN1  = 4;  
const int motorEN1  = 5;  
//правый мотор
const int motorIN2  = 2; 
const int motorEN2  = 3;  

void setup() {
Serial.begin (9600); 
  myservo.attach(9);  
  myservo.write(115); 
  delay(2000);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
}

void loop() {
 int distanceR = 0;
 int distanceL =  0;
 delay(40);
 
 if(distance<=20)
 {
  moveStop();
  delay(100);
  moveBackward();
  delay(300);
  moveStop();
  delay(200);
  distanceR = lookRight();
  delay(200);
  distanceL = lookLeft();
  delay(200);

  if(distanceR>=distanceL)
  {
    turnRight();
    moveStop();
  }else
  {
    turnLeft();
    moveStop();
  }
 }else
 {
  moveForward();
 }
 distance = readPing();
}

int lookRight()
{
    myservo.write(50); 
    delay(500);
    int distance = readPing();
    delay(100);
    myservo.write(115); 
    return distance;
}

int lookLeft()
{
    myservo.write(170); 
    delay(500);    
int distance = readPing();
    delay(100);
    myservo.write(115); 
    return distance;
    delay(100);
}

int readPing() { 
  delay(70);
  int cm = sonar.ping_cm();
  if(cm==0)
  {
    cm = 250;
  }
  return cm;
}

void moveStop() {
 // digitalWrite(motorIN1, HIGH);
    analogWrite(motorEN1, 0);
 //   digitalWrite(motorIN2, LOW);
    analogWrite(motorEN2, 0);
  } 
  
void moveForward() {


    digitalWrite(motorIN1, HIGH);
    analogWrite(motorEN1, 180);
    digitalWrite(motorIN2, HIGH);
    analogWrite(motorEN2, 180);  
  
}

void moveBackward() {
  
    digitalWrite(motorIN1, LOW);
    analogWrite(motorEN1, 180);
    digitalWrite(motorIN2, LOW);
    analogWrite(motorEN2, 180); 
    
  
}  

void turnRight() {
    digitalWrite(motorIN1, HIGH);
    analogWrite(motorEN1, 180);
    digitalWrite(motorIN2, LOW);
    analogWrite(motorEN2, 180);  
  delay(300);
 moveForward();      
  
} 
 
void turnLeft() {
    digitalWrite(motorIN1, LOW);
    analogWrite(motorEN1, 180);
    digitalWrite(motorIN2, HIGH);
    analogWrite(motorEN2, 180); 
  delay(300);
   moveForward();
}