Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <Servo.h>
- Servo VerticalServo;
- Servo GorizontalServo;
- int echoPin = 2;
- int trigPin = 3;
- #define ENA 4
- #define ENB 11
- #define IN1 7
- #define IN2 6
- #define IN3 5
- #define IN4 8
- #define RedIdeod 12
- void setup()
- {
- VerticalServo.attach(10);
- GorizontalServo.attach(9);
- VerticalServo.write(108);
- Serial.begin (9600);
- pinMode(trigPin, OUTPUT);
- pinMode(echoPin, INPUT);
- }
- void loop()
- {
- digitalWrite(IN1, HIGH);
- digitalWrite(IN2, LOW);
- analogWrite(ENA, 255);
- digitalWrite(IN3, HIGH);
- digitalWrite(IN4, LOW);
- analogWrite(ENB, 255);
- int leftDistance;
- int rightDistance;
- int duration, distation;
- digitalWrite(trigPin, LOW);
- delayMicroseconds(2);
- digitalWrite(trigPin, HIGH);
- delayMicroseconds(10);
- digitalWrite(trigPin, LOW);
- duration = pulseIn(echoPin, HIGH);
- distation = duration / 58;
- if (distation > 30) {
- GorizontalServo.write(56);
- }
- if ((distation > 0) && (distation < 300)) {
- Serial.print(distation);
- Serial.println(" cm");
- delay(100);
- }
- if (distation < 1) {
- Serial.print("Робот выключен, пожалуйста нажмите на выключатель ");
- Serial.println(" ");
- delay(100);
- }
- if ((distation < 30) && (distation > 0 )) {
- digitalWrite(IN1, LOW);
- digitalWrite(IN2, LOW);
- digitalWrite(IN3, LOW);
- digitalWrite(IN4, LOW);
- GorizontalServo.write(7);
- digitalWrite(trigPin, LOW);
- delayMicroseconds(2);
- digitalWrite(trigPin, HIGH);
- delayMicroseconds(10);
- digitalWrite(trigPin, LOW);
- rightDistance = pulseIn(echoPin, HIGH);
- rightDistance = rightDistance / 58;
- delay(1000);
- GorizontalServo.write(120);
- digitalWrite(trigPin, LOW);
- delayMicroseconds(2);
- digitalWrite(trigPin, HIGH);
- delayMicroseconds(10);
- digitalWrite(trigPin, LOW);
- leftDistance = pulseIn(echoPin, HIGH);
- leftDistance = leftDistance / 58;
- delay(1000);
- GorizontalServo.write(55);
- delay(1000);
- if (rightDistance > leftDistance) {
- digitalWrite(IN1, HIGH);
- digitalWrite(IN2, LOW);
- digitalWrite(IN3, LOW);
- digitalWrite(IN4, HIGH);
- delay(380);
- }
- if (rightDistance < leftDistance) {
- digitalWrite(IN1, HIGH);
- digitalWrite(IN2, LOW);
- digitalWrite(IN3, LOW);
- digitalWrite(IN4, HIGH);
- delay(380);
- }
- }
- if (distation == 0) {
- analogWrite(12, 255);
- delay(100);
- if (rightDistance == 0) {
- analogWrite(12, 255);
- delay(100);
- if (leftDistance == 0) {
- analogWrite(RedIdeod, 255);
- delay(100);
- }
- }
- }
- }
Add Comment
Please, Sign In to add comment