/* ESP32 WiFi Scanning example */
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "WiFi.h"
#include <ESP32Servo.h>


#define SCREEN_WIDTH 128 // OLED width,  in pixels
#define SCREEN_HEIGHT 64 // OLED height, in pixels

const int servoPin1 = 13;
const int servoPin2 = 12;
const int servoPin3 = 14;
const int servoPin4 = 27;

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;



// create an OLED display object connected to I2C
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

void setup() {
  Serial.begin(115200);
  Serial.println("Initializing WiFi...");
  WiFi.mode(WIFI_STA);
  WiFi.begin("Wokwi-GUEST", "");
  Serial.print("Connecting to WiFi ..");
  while (WiFi.status() != WL_CONNECTED) {
    Serial.print('.');
    delay(1000);
  }
  Serial.println("Your IP Address is: ");
  Serial.print(WiFi.localIP());

    // initialize OLED display with I2C address 0x3C
  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("failed to start SSD1306 OLED"));
    while (1);
  }

  delay(2000);         // wait two seconds for initializing
  oled.clearDisplay(); // clear display

  oled.setTextSize(1);         // set text size
  oled.setTextColor(WHITE);    // set text color
  oled.setCursor(0, 2);       // set position to display (x,y)
  oled.println("Robotronix"); // set text
  oled.display();              // display on OLED

  oled.setTextSize(1);         // set text size
  oled.setTextColor(WHITE);    // set text color
  oled.setCursor(0, 2);       // set position to display (x,y)
  oled.println("Testing for pill 1"); // set text
  oled.display();              // display on OLED
  pill1();

  oled.setTextSize(1);         // set text size
  oled.setTextColor(WHITE);    // set text color
  oled.setCursor(0, 2);       // set position to display (x,y)
  oled.println("Testing for pill 2"); // set text
  oled.display();              // display on OLED
  pill2();

}

void loop() {
  // Wait a bit before scanning again
  delay(5000);
}

void pill1(){
  servo1.attach(servoPin1, 500, 2400);
  servo1.write(90);
  delay(500);
  servo1.write(0);
  delay(500);
  servo1.write(90);
  delay(500);

  servo2.attach(servoPin2, 500, 2400);
  servo2.write(90);
  delay(500);
  servo2.write(0);
  delay(500);
  servo2.write(90);
  delay(500);
}

void pill2(){
  servo3.attach(servoPin3, 500, 2400);
  servo3.write(90);
  delay(500);
  servo3.write(0);
  delay(500);
  servo3.write(90);
  delay(500);

  servo4.attach(servoPin4, 500, 2400);
  servo4.write(90);
  delay(500);
  servo4.write(0);
  delay(500);
  servo4.write(90);
  delay(500);
}