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

///////////ตั้งค่าปุ่มกด///////////////////

int button =  34; /// กำหนดปุ่มกดสวิตซ์ขา 27

int snd = 12;

int ss1 = 23;  // เปิดใช้งาน s1 ที่พอร์ต 19

int ss2 = 19;  // เปิดใช้งาน s1 ที่พอร์ต 23

int ss3 = 18;  // เปิดใช้งาน s1 ที่พอร์ต 18

Servo sv1;     // ประกาศการใช้งาน sv1

Servo sv2;     // ประกาศการใช้งาน sv2

Servo sv3;     // ประกาศการใช้งาน sv3

///////////////////////องศา เซอร์โว///////////////////////

const int v_before = 150 ;      //กำหนดค่ายกมือขึ้น servo 180

const int v_after = 40;    //กำหนดค่ายกมือลง servo 0

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

#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 (ขวากลาง)

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

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

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

int s0,s1,s2,s3,s4,s5 ; 

void analogs()  /////จะให้ดี ใช้ 39 36

{

  s0 = analogRead(14);

  s1 = analogRead(25);

  s2 = analogRead(26);

  s3 = analogRead(27);

  s4 = analogRead(32);

  s5 = analogRead(33);

}

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

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

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

  pinMode(s5, 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);  

  pinMode(ss1, OUTPUT);

  pinMode(ss2, OUTPUT);

  pinMode(ss3, OUTPUT);

  sv1.attach(ss1); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv1

  sv2.attach(ss2); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv2

  sv3.attach(ss3); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv3

  sv_set();

}

void loop() {

  int sw = digitalRead(button);     // ให้ sw อ่านค่า digital จากพอร์ต 2(button)

  int nob = analogRead(35);          // ให้ nob เทียบเท่าค่า 35

  int menu = map(nob, 0, 4094, 0, 9); // เทียบอัตราส่วนของพอร์ต 35 จาก 0-4095 เพื่อทำเป็นเมนู 0-9

  display.clearDisplay();              // ล้างค่าหน้าจอ

  display.setTextColor(WHITE, BLACK);  //สีอักษรเป็นสีขาว ,พื้นหลังดำ

  display.setCursor(0, 0);       // เซตตำแหน่ง 0,0

  display.setTextSize(2);        // เซตขนาดอักษรมีขนาดเป็น 2

  display.print("   ");         // วรรค

  display.println(menu);        // แสดงค่า Menu ที่ได้จากการ map nob ให้เหลือ 0-9

  display.setTextSize(1);        // เซตขนาดอักษรมีขนาดเป็น 2

  display.println("  Krunote");     // พิมพ์คำว่า kruro

  display.print("  ");                      // วรรค

  display.print(nob);                     // แสดงค่าที่อ่านได้จาก nob หรือ 35

  display.println(" Robot");     // พิมพ์คำว่า Robot

  display.display();

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

  if ((sw == 1) and (menu == 0))

  { 

    sensor();

  }

  if ((sw == 1) and (menu == 1))

  {

    sv_knob();

  }

  if ((sw == 1) and (menu == 2))

  { 

    menu2();

  }

  if ((sw == 1) and (menu == 3))

  {

    menu3();

  }

  if ((sw == 1) and (menu == 4))

  {

   menu4();

  }

  if ((sw == 1) and (menu == 5))

  {

   menu5();

  }

  if ((sw == 1) and (menu == 6))

  {

  menu6();

  }

  if ((sw == 1) and (menu == 7))

  {

  menu7();

  }

  if ((sw == 1) and (menu == 8))

  {

  menu8();

  }

  if ((sw == 1) and (menu == 9))

  {

  menu9();

  }

  delay(100);

}

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

void sensor()

{

  while (true) {

      analogs();

      display.clearDisplay();

      display.setTextColor(WHITE, BLACK);  //สีอักษรเป็นสีขาว ,พื้นหลังดำ

      display.setCursor(0, 0);       // เซตตำแหน่ง 0,0

      display.setTextSize(1);        // เซตขนาดอักษรมีขนาดเป็น 1

      display.print("       S0 = "); display.println(s0);  // แสดงค่าเซนเซอร์ S0

      display.print("       S1 = "); display.println(s1);  // แสดงค่าเซนเซอร์ S1

      display.print("       S2 = "); display.println(s2);  // แสดงค่าเซนเซอร์ S2

      display.print("       S3 = "); display.println(s3);  // แสดงค่าเซนเซอร์ S3

      display.print("       S4 = "); display.println(s4);  // แสดงค่าเซนเซอร์ S2

      display.print("       S5 = "); display.println(s5);  // แสดงค่าเซนเซอร์ S3

      display.display();

      delay(50);

    }

  }

/////////////////////////////ฟังก์ชันเซอร์โว////////////////////////////////////////

void sv_knob() {

  while(true){

  int vr = analogRead(35);  // กำหนดตัวแปรจำนวนเต็มอ่านค่าอนาล็อกที่พอร์ต 35

  int nob = map(vr, 0, 4096, 0, 180); // ทำการ map อัตราส่วนจากสัญญาณ analog 0-1023 เป็น 0-180

  display.clearDisplay();    // เคลียร์หน้าจอ oled

  display.setTextColor(WHITE, BLACK);  //สีอักษรเป็นสีขาว ,พื้นหลังดำ

  display.setCursor(0, 0);       // เซตตำแหน่ง 0,0

  display.setTextSize(1);        // เซตขนาดอักษรมีขนาดเป็น 2

  display.print("Servo1 = ");     // พิมพ์คำว่า Servo1 = 

  display.print("Servo2 = ");     // พิมพ์คำว่า Servo2 = 

  display.print("Servo3 = ");     // พิมพ์คำว่า Servo3 = 

  display.println(nob);     // นำค่า nob มาแสดงใน oled

  display.display();        // เปิดฟังก์ชันแสดงผล

  sv1.write(nob);        // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้

  sv2.write(nob);        // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้

  sv3.write(nob);        // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้

  delay(50);             // หน่วงเวลา 0.05 วินาที

  }

}

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

void sv_set()   // เซตเซอร์โว

{

  sv1.write(90);

  sv2.write(90);

  sv3.write(90);// ค่าองศาเซอร์โวตอนเปิดบอร์ด

  }

/////////////////////////ฟังก์ชันเส้นทาง////////////////////////////////////////

void P()     ///////////// ฟังก์ชันแทกเส้นเจอคู่ดำหยุด/////////////////////////////

{while (1){ 

  analogs();

if ((s0 > A0) and (s3 > A3)){run(100, 100);}

if (s0 < A0){run(100, -100);}

if (s3 < A3){run(-100, 100);}

if (((s1 < A1) and (s2 < A2)) or ((s1 < A1) and (s0 < A0)) or ((s2 < A2) and (s3 < A3)))

    {run(0, 0);delay(100); run(-100, -100);delay(180);run(0, 0);delay(100);break;}}}

void T() //ตัวหลักสำหรับแก้ความเร็วบนเส้นทางท้งหมด

{

  analogs();    

  if ((s0 > A0) and (s3 > A3)) {run(100, 100);}

    if (s0 < A0) {run(100, -100);}

    if (s3 < A3) {run(-100, 100);}}

void B() //ตัวหลักสำหรับแก้ความเร็วบนเส้นทางท้งหมด

{while(1){

  analogs();    

  if ((s4 > A4) and (s5 > A5)) {run(-50, -50);}

    if (s4 < A4) {run(100, -100);}

    if (s5 < A5) {run(-100, 100);}

    if ((s4 < A4) and (s5 < A5)) {run(0,0);break;}}}

void PT() //pass เส้นT ==ถ้าเซนเซอร์ทั้ง 2 ตัวเจอดำ หยุด==

{while (1){

  T();

  if ((s1 < A1)&&(s2 < A2)){run(-80,-80); delay(275);

       run(0,0); delay(100); break;}}}

void PD(int times) //วิ่งไปเรื่อยๆจนกว่าจะหมดเวลา

{int i=0;

  while (1){

  T();

  if (i>times){run(0,0); delay(500);  break;}

  i++;

    }}

void PL() //passleave ==ถ้าเซนเซอร์ทั้ง 4 ตัวเจอดำ หยุด เพื่อที่จะทิ้งถุงยังชีพ==

{while (1){

  T();

  if ((s1 < A1)&&(s2 < A2)){run(0,0); delay(100); break;}}}

void ST(int T) { //กรณีที่จุดstartเป็นพื้นที่ที่เซ็นเซอร์ไม่สามารถวัดค่าแบบตายตัวได้

 run(100,100);delay(T); run(0,0); delay(10);

}

void Fall(int T) { //กรณีที่จุดstartเป็นพื้นที่ที่เซ็นเซอร์ไม่สามารถวัดค่าแบบตายตัวได้

 run(-130,-130);delay(T); run(0,0); delay(10);

}

/////////////////////////เลี้ยวซ้าย///////////////////////////////////////////

void LD(int T) { //เลี้ยวซ้ายโดยไม่สนเซ็นเซอร์แต่ใช้การdelayเอา

 run(-130,130);delay(T); run(0,0); delay(10);

}

//////////////////////เลี้ยวขวา//////////////////////////////////////////

void RD(int T) { //เลี้ยวขวาโดยไม่สนเซ็นเซอร์แต่ใช้การdelayเอา

 run(130,-130);delay(T); run(0,0); delay(10);

}

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

void b(){run(-100,-100);delay(1000);run(0,0);delay(600);} // ถอยหลัง

/////////////////////// ฟังก์ชันเซฮร์โว //////////////////////////

void leave() {

sv1.write(v_before);delay(500);

sv2.write(v_before);delay(500);

sv3.write(v_before);delay(500);

sv1.write(v_after);delay(500); 

sv2.write(v_after);delay(500); 

sv3.write(v_after);delay(500); 

}

  void up(){

    sv1.write(v_before);

    sv2.write(v_before);

    sv3.write(v_before);

    delay(1000); } // เซอร์โวยก

  void down(){ 

    sv1.write(v_after);

    sv2.write(v_after);

    sv3.write(v_after);

    delay(1000); }// เซอร์โววาง

//////////////////////////////// เรียกใช้ code menu ////////////

void menu2()   /// code 2 ที่นี่

{

run(100,100);delay(1000);run(-100,100);delay(1000);run(100,-100);delay(1000);run(-100,-100);delay(1000);run(0,0);delay(1000);

}

void menu3()   /// code 3 ที่นี่

{

 ST(500);PT();LD(420);PT();LD(420);B();PT();LD(420);B();PT();RD(420);B();PL();leave();run(-100,-100);delay(600);

 RD(420);B();PT();RD(420);PT();LD(420);B();PT();LD(420);B();PT();LD(420);B();PL();leave();

 run(-100,-100);delay(500);B();LD(410);run(100,100);delay(1800);run(0,0);delay(50);LD(420);B();PT();RD(420);PL();leave();

 LD(800);B();PL();leave();run(-100,-100);delay(1500);run(0,0);delay(50);LD(420);B();PT();LD(420);PT();RD(420);B();PT();RD(420);

 B();run(100,100);delay(2000);run(0,0);delay(50);LD(390);B();PT();LD(420);PT();LD(420);B();PT();RD(420);B();PT();RD(420);B();

 PT();RD(420);B();PT();run(100,100);delay(700);run(0,0);delay(1000000000000000);

}

void menu4()   /// code 4 ที่นี่

{

}

void menu5()   /// code 5 ที่นี่

{

}

void menu6()   /// code 6 ที่นี่

{

  b();

}

void menu7()   /// code 7 ที่นี่

{

  up();

}

void menu8()   /// code 8 ที่นี่

{

  down();

}

void menu9()   /// code 9 ที่นี่

{

}

/////////////////////////////ฟังก์ชันมอเตอร์///////////////////////////////////

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, HIGH);

    digitalWrite(DL2, HIGH);

  }

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

  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, HIGH);

    digitalWrite(DR2, HIGH);

  }

}