From 991f33c02f55c6334a65ef753e25a7fdb4667718 Mon Sep 17 00:00:00 2001 From: "vojta@viky" Date: Tue, 7 Apr 2026 17:26:57 +0200 Subject: [PATCH] Zaloha - Davidovo automaticke auto. --- zalohy/auto_david.ino | 165 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 165 insertions(+) create mode 100644 zalohy/auto_david.ino diff --git a/zalohy/auto_david.ino b/zalohy/auto_david.ino new file mode 100644 index 0000000..a8175e7 --- /dev/null +++ b/zalohy/auto_david.ino @@ -0,0 +1,165 @@ + +#include +#include +Servo myservo; +int Echo_Pin=A0; // ultrasonic module ECHO to A0 +int Trig_Pin=A1; // ultrasonic module TRIG to A1 +#define Lpwm_pin 5 //pin of controlling speed---- ENA of motor driver board +#define Rpwm_pin 6 //pin of controlling speed---- ENB of motor driver board +int pinLB=2; //pin of controlling turning---- IN1 of motor driver board +int pinLF=4; //pin of controlling turning---- IN2 of motor driver board +int pinRB=7; //pin of controlling turning---- IN3 of motor driver board +int pinRF=8; //pin of controlling turning---- IN4 of motor driver board +volatile int D_mix; +volatile int D_mid; +volatile int D_max; +volatile int Front_Distance; +volatile int Left_Distance; +volatile int Right_Distance; +volatile int Right_IR_Value; +volatile int Left_IR_Value; + +float checkdistance() { + digitalWrite(Trig_Pin, LOW); + delayMicroseconds(2); + digitalWrite(Trig_Pin, HIGH); + delayMicroseconds(10); + digitalWrite(Trig_Pin, LOW); + float distance = pulseIn(Echo_Pin, HIGH) / 58.00; + delay(10); + return distance; +} + +void Detect_Left_and_Right__distance() { + myservo.write(180); + delay(400); + Left_Distance = checkdistance(); + delay(600); + Serial.print("Left_Distance:"); + Serial.println(Left_Distance); + myservo.write(0); + delay(400); + Right_Distance = checkdistance(); + delay(600); + Serial.print("Right_Distance:"); + Serial.println(Right_Distance); + myservo.write(90); +} + +void Ultrasonic_obstacle_avoidance() +{ + Front_Distance=checkdistance(); //obtain the value detected by ultrasonic sensor + if((Front_Distance < 20)&&(Front_Distance > 0)) { + stopp();//stop + delay(100); + myservo.write(180); + delay(500); + Left_Distance=checkdistance();//measure the distance + delay(100); + myservo.write(0); + delay(500); + Right_Distance=checkdistance();//measure the distance + delay(100); + if(Left_Distance > Right_Distance) { + rotate_left(150);//turn left + myservo.write(90); + delay(300); + } + else { + rotate_right(150);// turn right + myservo.write(90); + delay(300); + } + } + else { + go_forward(100);//go forward + } +} + + + + + + +void Obstacle_Avoidance_Main() +{ + Ultrasonic_obstacle_avoidance(); +} + + + +void setup(){ + myservo.attach(A2); + Serial.begin(9600); + D_mix = 10; + D_mid = 20; + D_max = 100; + Front_Distance = 0; + Left_Distance = 0; + Right_Distance = 0; + myservo.write(90); + pinMode(Echo_Pin, INPUT); + pinMode(Trig_Pin, OUTPUT); + pinMode(pinLB,OUTPUT); // /pin 2 + pinMode(pinLF,OUTPUT); // pin 4 + pinMode(pinRB,OUTPUT); // pin 7 + pinMode(pinRF,OUTPUT); // pin 8 + pinMode(Lpwm_pin,OUTPUT); // pin 5 (PWM) + pinMode(Rpwm_pin,OUTPUT); // pin 6(PWM) +} + +void loop(){ + Obstacle_Avoidance_Main(); + +} + + + +void go_forward(unsigned char speed_val) // speed_val：0~255 + {digitalWrite(pinRB,HIGH); + digitalWrite(pinRF,LOW); + digitalWrite(pinLB,HIGH); + digitalWrite(pinLF,LOW); + analogWrite(Lpwm_pin,speed_val+6); + analogWrite(Rpwm_pin,speed_val+5); + + + } + +void go_backward(unsigned char speed_val) // speed_val：0~255 + { + digitalWrite(pinRB,LOW); + digitalWrite(pinRF,HIGH); + digitalWrite(pinLB,LOW); + digitalWrite(pinLF,HIGH); + analogWrite(Lpwm_pin,speed_val); + analogWrite(Rpwm_pin,speed_val); + } + +void rotate_left(unsigned char speed_val) // speed_val：0~255 + {digitalWrite(pinRB,HIGH); + digitalWrite(pinRF,LOW ); + digitalWrite(pinLB,LOW); + digitalWrite(pinLF,HIGH); + analogWrite(Lpwm_pin,speed_val); + analogWrite(Rpwm_pin,speed_val); + + + } +void rotate_right(unsigned char speed_val) // speed_val：0~255 + { + digitalWrite(pinRB,LOW); + digitalWrite(pinRF,HIGH); + digitalWrite(pinLB,HIGH); + digitalWrite(pinLF,LOW); + analogWrite(Lpwm_pin,speed_val); + analogWrite(Rpwm_pin,speed_val); + + } +void stopp() //stop + { + digitalWrite(pinRB,HIGH); + digitalWrite(pinRF,HIGH); + digitalWrite(pinLB,HIGH); + digitalWrite(pinLF,HIGH); + }