#include <Servo.h>

const int ledPin = 21;   // LED connected to digital pin 21
const int motorPin = 9;  // Motor (LED) connected to digital pin 9
const int servoPin = 10; // Servo motor connected to digital pin 10

Servo servoMotor; // Create a servo object to control the servo motor

void setup() {
  pinMode(ledPin, OUTPUT);       // Initialize the LED pin as an output
  pinMode(motorPin, OUTPUT);     // Initialize the motor (LED) pin as an output
  servoMotor.attach(servoPin);   // Attach the servo motor to its pin
}

void loop() {
  // Turn LED (motor) on (simulate Pixhawk command)
  digitalWrite(ledPin, HIGH);
  Serial.println("Sending command: LED (MOTOR) ON");

  // Turn motor (LED) off (simulate Pixhawk command)
  digitalWrite(motorPin, LOW);
  Serial.println("Sending command: MOTOR (LED) OFF");

  // Move servo motor to one position (simulate drone action)
  servoMotor.write(90); // Set servo motor to 90 degrees
  Serial.println("Sending command: SERVO 90");
  delay(1000);          // Wait for 1 second

  // Move servo motor to another position (simulate drone action)
  servoMotor.write(180); // Set servo motor to 180 degrees
  Serial.println("Sending command: SERVO 180");
  delay(1000);           // Wait for 1 second

  // Turn LED (motor) off (simulate Pixhawk command)
  digitalWrite(ledPin, LOW);
  Serial.println("Sending command: LED (MOTOR) OFF");

  // Turn motor (LED) on (simulate Pixhawk command)
  digitalWrite(motorPin, HIGH);
  Serial.println("Sending command: MOTOR (LED) ON");

  delay(1000); // Wait for 1 second before repeating
}