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