#include <Arduino.h>
#include <ESP32Servo.h>
// Servo bên trái
Servo leftServo;
// Servo bên phải
Servo rightServo;
// Góc lệch dùng để mô phỏng tốc độ
int speedValue = 45;
// =========================
// Robot đi tới
// =========================
void forward() {
// Hai servo nghiêng ngược chiều
// để mô phỏng robot chạy tới
leftServo.write(135);
rightServo.write(45);
Serial.println("Robot moving forward");
}
// =========================
// Robot đi lùi
// =========================
void backward() {
leftServo.write(45);
rightServo.write(135);
Serial.println("Robot moving backward");
}
// =========================
// Robot rẽ trái
// =========================
void turnLeft() {
leftServo.write(45);
rightServo.write(45);
Serial.println("Robot turning left");
}
// =========================
// Robot rẽ phải
// =========================
void turnRight() {
leftServo.write(135);
rightServo.write(135);
Serial.println("Robot turning right");
}
// =========================
// Robot dừng
// =========================
void stopRobot() {
leftServo.write(90);
rightServo.write(90);
Serial.println("Robot stopped");
}
void setup() {
Serial.begin(115200);
leftServo.attach(18);
rightServo.attach(19);
stopRobot();
Serial.println("===== ESP32 ROBOT CONTROL =====");
Serial.println("F = Forward");
Serial.println("B = Backward");
Serial.println("L = Left");
Serial.println("R = Right");
Serial.println("S = Stop");
}
void loop() {
if (Serial.available()) {
char cmd = Serial.read();
if (cmd == 'F' || cmd == 'f') {
forward();
}
else if (cmd == 'B' || cmd == 'b') {
backward();
}
else if (cmd == 'L' || cmd == 'l') {
turnLeft();
}
else if (cmd == 'R' || cmd == 'r') {
turnRight();
}
else if (cmd == 'S' || cmd == 's') {
stopRobot();
}
}
}