/*
  ESP32 Servo
  Use LED on GPIO5
  Use Servo on GPIO2
  Use Buzer on GPIO 4
*/
#include <ESP32Servo.h>
#include<Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
Adafruit_SSD1306 OLED(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
#define ServoPin 2  // Define the pin position of the Servo
#define ledPin  5    // Led Pin  (GPIO5)
#define Buzer  4    // Buzer pin (GPIO4)
Servo my_Servo;     // Create an object of the servo 


void setup()
{
  // Using I2C check if the address of connection corrrect
  if(!OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  delay(50);
  //Text configuration on the display
  OLED.clearDisplay();                // Clear Screen
  OLED.setTextSize(1.5);             // Set text size
  OLED.setTextColor(WHITE);         // Set Text color
  OLED.setCursor(0, 0);            //Set cursor
  // Display a static text 
  OLED.println("Hello and welcome !");
  OLED.display(); 
  delay(1000);
  // Set LED as output
  pinMode(ledPin, OUTPUT);     // Set the LED pin as ouput
  pinMode(Buzer, OUTPUT);      // Set the buzer as an output
  my_Servo.attach(ServoPin);   // Set the Servo by attaching its pin to the object my_Servo
  
  // Serial monitor setup
  Serial.begin(115200);
}

void loop()
{
   
    for(int pos=0; pos<=180; pos+=10){   // Move the horn og the Servo from to 0 to 180 with a step of 20
       my_Servo.write(pos);
       OLED.clearDisplay();
       OLED.setCursor(0, 3);
       OLED.print(pos);                // Dispaly the angle of the servo on the OLED
       OLED.println(" degree!");
       OLED.display();
       if(digitalRead(ServoPin)==0){         // If the servo moves light up the LED
          digitalWrite(ledPin, HIGH);
       }
       tone(Buzer, 5*pos+200, 10);       // Emit a tone from the buzer correcponding to the Servo position
       delay(250);
      digitalWrite(ledPin, LOW);
      delay(100);

     }
     
}