library/Arduino/esp32_servo_sonic/esp32_servo_sonic.ino

105 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);
}