#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(" KruRo"); // พิมพ์คำว่า 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);
}
}