//////// ผนวกไลบรารี่ ////////////

#include <ESP32Servo.h> 

#include <SPI.h>

#include <Wire.h>

#include <Adafruit_GFX.h>

#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels

#define SCREEN_HEIGHT 64 // OLED display height, in pixels

#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)

#define SCREEN_ADDRESS 0x3C  ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

////////////ตั้งค่าพอร์ตมอเตอร์////////////////////

#define DL1  2  /// กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 15

#define DL2  15  /// กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 2

#define PWML 13 /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 13

/////////////////////////////////

#define DR1  17  // กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 16

#define DR2  16  // กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 17

#define PWMR 4  /// กำหนดสัญญาณ PWM ขวาพอร์ต 4

/////////////////ตั้งค่าพอร์ต Analog//////////////

#define A0 2000     // ค่ากลางเซนเซอร์ A0 (ซ้ายสุด)

#define A1 2000      // ค่ากลางเซนเซอร์ A1 (ซ้ายกลาง)

#define A2 2000       // ค่ากลางเซนเซอร์ A2 (กลาง)

#define A3 2000      // ค่ากลางเซนเซอร์ A3 (ขวากลาง)

///////////////////////////////////////////////

int button =  34;

int s0,s1,s2,s3 ; 

void setup() {

  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS); // กำหนดแอดเดรสของพอร์ตจอเป็น 0x3C (for the 128x64)

  pinMode(34, INPUT); // ตั้งค่าขา 34 เป็น INPUT  

  pinMode(s0, INPUT); // ตั้งค่าขา s0 เป็น INPUT

  pinMode(s1, INPUT); // ตั้งค่าขา s1 เป็น INPUT

  pinMode(s2, INPUT); // ตั้งค่าขา s2 เป็น INPUT

  pinMode(s3, INPUT); // ตั้งค่าขา s3 เป็น INPUT

  ////////กำหนดสัญญาณทุกขาเป็น OUTPUT /////////

  pinMode(12, OUTPUT);  

  pinMode(DL1, OUTPUT);

  pinMode(DL2, OUTPUT);

  pinMode(PWML, OUTPUT);

  pinMode(DR1, OUTPUT);

  pinMode(DR2, OUTPUT);

  pinMode(PWMR, OUTPUT); 

}

void loop() {

   int sw = digitalRead(button);

if (sw==1){while(true){

  /////////// ตั้งค่าเซนเซอร์ตามการอ่านพอร์ตอนาล็อก S0,S1,S2,S3

  int s0 = analogRead(14);

  int s1 = analogRead(25);

  int s2 = analogRead(26);

  int s3 = analogRead(27);

if (s0 < A0){run(-75, 75);delay(120);}

if (s1 < A1){run(0, 75);delay(120);}

if (s2 < A2){run(75, 0);delay(60);}

if (s3 < A3){run(75, -75);delay(60);}

  else {run(50,50);delay(20);}

}

}

}

void run(int spl, int spr)   // ประกาศฟังก์ชัน run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);

{

  if (spl > 0)

  {

    digitalWrite(DL1, LOW);

    digitalWrite(DL2, HIGH);

    analogWrite(PWML, spl);

  }

  else if (spl < 0)

  {

    digitalWrite(DL1, HIGH);

    digitalWrite(DL2, LOW);

    analogWrite(PWML, -spl);

  }

  else

  {

    digitalWrite(DL1, LOW);

    digitalWrite(DL2, LOW);

  }

  //////////////////////////////////////

  if (spr > 0)

  {

    digitalWrite(DR1, LOW);

    digitalWrite(DR2, HIGH);

    analogWrite(PWMR, spr);

  }

  else if (spr < 0)

  {

    digitalWrite(DR1, HIGH);

    digitalWrite(DR2, LOW);

    analogWrite(PWMR, -spr);

  }

  else

  {

    digitalWrite(DR1, LOW);

    digitalWrite(DR2, LOW);

  }

}