106 lines
3.5 KiB
C++
106 lines
3.5 KiB
C++
#include <ESP32Servo.h>
|
|
|
|
Servo myservo; // create servo object to control a servo
|
|
int servo_angle = 0; // variable to store the servo position
|
|
#define SERVOPIN 16 // Recommended PWM GPIO pins on the ESP32 include 2,4,12-19,21-23,25-27,32-33
|
|
#define SERVO_LEFT 45
|
|
#define SERVO_RIGHT 135
|
|
#define SERVO_FORWARD 90
|
|
|
|
|
|
#define US_TRIG 12
|
|
#define US_ECHO 14
|
|
#define US_STOP_DISTANCE 10
|
|
long distance_forward_cm,distance_left_cm,distance_right_cm;
|
|
|
|
|
|
void setup() {
|
|
//Serial port
|
|
Serial.begin(115200);
|
|
|
|
//Ultra sonic pins
|
|
pinMode(US_TRIG, OUTPUT);
|
|
pinMode(US_ECHO, INPUT);
|
|
//servo init
|
|
myservo.attach(SERVOPIN); // attaches the servo on pin 18 to the servo object
|
|
// using default min/max of 1000us and 2000us
|
|
// different servos may require different min/max settings
|
|
// for an accurate 0 to 180 sweep
|
|
delay(1000);
|
|
myservo.write(SERVO_FORWARD); // start looking forward
|
|
delay(2000);
|
|
myservo.write(SERVO_LEFT); // start looking forward
|
|
delay(2000);
|
|
myservo.write(SERVO_FORWARD); // start looking forward
|
|
delay(2000);
|
|
myservo.write(SERVO_RIGHT); // start looking forward
|
|
delay(2000);
|
|
myservo.write(SERVO_FORWARD); // start looking forward
|
|
|
|
}
|
|
|
|
|
|
void loop() {
|
|
|
|
distance_forward_cm = measure_distance_cm(); // measure the distance in front of us
|
|
|
|
if(distance_forward_cm<US_STOP_DISTANCE){
|
|
//there is something in front of us
|
|
myservo.write(SERVO_LEFT); // look the left and meaasure distance
|
|
delay(1000); // wait 1s
|
|
distance_left_cm = measure_distance_cm(); // measure the distance in left of us
|
|
delay(1000); // wait 1s
|
|
myservo.write(SERVO_RIGHT); // look the left and meaasure distance
|
|
delay(1000); // wait 1s
|
|
distance_right_cm = measure_distance_cm(); // measure the distance in left of us
|
|
delay(1000); // wait 1s
|
|
|
|
if(distance_left_cm > distance_right_cm){
|
|
// goto to the left
|
|
for(int pos = SERVO_FORWARD;pos>SERVO_LEFT;pos--){
|
|
myservo.write(pos);
|
|
delay(15);
|
|
}
|
|
}
|
|
else{
|
|
// goto to the right
|
|
for(int pos = SERVO_FORWARD;pos<SERVO_RIGHT;pos++){
|
|
myservo.write(pos);
|
|
delay(15);
|
|
}
|
|
}
|
|
|
|
delay(1000);
|
|
myservo.write(SERVO_FORWARD);
|
|
delay(1000);
|
|
}
|
|
|
|
delay(250);//ms
|
|
}
|
|
|
|
long measure_distance_cm(){
|
|
long distance_cm,echo_duration;
|
|
// start measurment by a short high pulse of 10 microseconds length at the trig pin ...
|
|
digitalWrite(US_TRIG,LOW);
|
|
delayMicroseconds(2);
|
|
digitalWrite(US_TRIG,HIGH);
|
|
delayMicroseconds(10);
|
|
digitalWrite(US_TRIG,LOW);
|
|
|
|
// measure the duration in microseconds of the echo pin HIGH - this is the time the echo needs to be back at the sensor
|
|
pinMode(US_ECHO,INPUT);
|
|
echo_duration = pulseIn(US_ECHO,HIGH);
|
|
|
|
// convert into cm ... 344m/sec is the speed of noise - thus 34400cm/sec ... or 34,400cm/milisec ... or 0,0344cm/microsec
|
|
// the echo has to go the distance twice - forth and back - so the duration has to be the half of the measured one
|
|
// distance_cm = echo_duration/2 * 0,0344 or distance_cm = echo_duration/2 / 29,1
|
|
distance_cm = (echo_duration/2) / 29.1;
|
|
|
|
//print this result at the screen
|
|
Serial.print("Distance in cm: ");
|
|
Serial.println(distance_cm);
|
|
return(distance_cm);
|
|
|
|
}
|
|
|