#include <Arduino.h>
#include <Servo.h>
// =========================
// Khởi tạo 2 servo
// =========================
Servo leftServo;
Servo rightServo;
// =========================
// Robot đi tới
// =========================
void forward() {
// Servo trái nghiêng sang phải
// Servo phải nghiêng sang trái
// để 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() {
// 90 độ là vị trí trung tâm
// dùng để mô phỏng robot đứng yên
leftServo.write(90);
rightServo.write(90);
Serial.println("Robot stopped");
}
// =========================
// Hàm setup
// =========================
void setup() {
Serial.begin(115200);
// Gắn servo vào GPIO của Pico
leftServo.attach(2);
rightServo.attach(28);
// Dừng robot khi khởi động
stopRobot();
delay(1000);
}
// =========================
// Loop tự động mô phỏng
// =========================
void loop() {
// =====================
// Robot đi tới
// =====================
forward();
delay(2000);
// =====================
// Robot đi lùi
// =====================
backward();
delay(2000);
// =====================
// Robot rẽ trái
// =====================
turnLeft();
delay(2000);
// =====================
// Robot rẽ phải
// =====================
turnRight();
delay(2000);
// =====================
// Robot dừng
// =====================
stopRobot();
delay(2000);
}