#include <SPI.h>
#include <Wire.h>
#include <Servo.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 OLED(-1);
/////// motor R ////////
#define DR1 7 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 7
#define DR2 8 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 8
#define PWMR 6 /// กำหนดสัญญาณ PWM ขวาพอร์ต 6
/////////////////////////////////
#define DL1 4 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 9
#define DL2 9 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 4
#define PWML 5 /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 5
const int button = 2;
////////ตัวแปรเก็บค่าแสงจากเซนเซอร์
int S0;
int S1;
int S2;
int S3;
int S4;
//////////ค่ากลางเซนเซอร์ เอาไว้เปนียบเทียบค่าแสง//////////
int cen0 = 450;
int cen1 = 450;
int cen2 = 450;
int cen3 = 450;
int cen4 = 450;
///////////////////////องศา เซอร์โว///////////////////////
const int v_before = 67; //กำหนดค่ายกมือขึ้น servo 0
const int v_after = 0; //กำหนดค่ายกมือลง servo 0
///////////////ตั้งพอร์ตเซอร์โว////////////////////////////////
int servo1 = 10; // เปิดใช้งาน s1 ที่พอร์ต 10 zackrobot & 7 kruroRobot
int servo2 = 11; // เปิดใช้งาน s1 ที่พอร์ต 11 & 8
int servo3 = 12; // เปิดใช้งาน s1 ที่พอร์ต 12
Servo sv1; // ประกาศการใช้งาน sv1
Servo sv2; // ประกาศการใช้งาน sv2
Servo sv3; // ประกาศการใช้งาน sv3
void setup() {
OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C); // กำหนดแอดเดรสของพอร์ตจอเป็น 0x3C (for the 128x64)
pinMode(DL1, OUTPUT);
pinMode(DL2, OUTPUT);
pinMode(PWML, OUTPUT);
pinMode(DR1, OUTPUT);
pinMode(DR2, OUTPUT);
pinMode(PWMR, OUTPUT);
pinMode(button, INPUT);
///////////// ตั้งพอร์ตเซอร์โว////////////
sv1.attach(servo1); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv1
sv2.attach(servo2); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv2
sv3.attach(servo3); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv3
leave();
}
void set_servo() {
while (1) {
int vr = analogRead(A7); // กำหนดตัวแปรจำนวนเต็มอ่านค่าอนาล็อกที่พอร์ต 7
int nob = map(vr, 0, 1023, 0, 180); // ทำการ map อัตราส่วนจากสัญญาณ analog 0-1023 เป็น 0-180
OLED.clearDisplay(); // เคลียร์หน้าจอ oled
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0
OLED.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.println(nob); // นำค่า nob มาแสดงใน oled
OLED.display(); // เปิดฟังก์ชันแสดงผล
sv1.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
sv2.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
sv3.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
delay(50); // หน่วงเวลา 0.05 วินาที
}
}
void analogs() {
S0 = analogRead(0); // กำหนดตัวแปรเพื่อเก็บค่า ADC Ch0
S1 = analogRead(1); // กำหนดตัวแปรเพื่อเก็บค่า ADC Ch1
S2 = analogRead(2); // กำหนดตัวแปรเพื่อเก็บค่า ADC Ch2
S3 = analogRead(3); // กำหนดตัวแปรเพื่อเก็บค่า ADC Ch3
S4 = analogRead(6); // กำหนดตัวแปรเพื่อเก็บค่า ADC Ch4
}
void sensor() {
while (true) {
analogs();
OLED.clearDisplay(); //ล้างหน้าจอ
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ จอที่ใช้ไม่สามารถเปลี่ยนสีได้
OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0
OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.print(" S0 : "); // แสดงข้อความ S0 :
OLED.print(S0, DEC); // แสดงค่า S0
OLED.print(" S1 : "); // แสดงข้อความ S1 :
OLED.println(S1, DEC); // แสดงค่า S1
OLED.print(" S2 : "); // แสดงข้อความ S2 :
OLED.println(S2, DEC); // แสดงค่า S2
OLED.print(" S3 : "); // แสดงข้อความ S3 :
OLED.println(S3, DEC); // แสดงค่า S3
OLED.print(" S4 : "); // แสดงข้อความ S4 :
OLED.println(S4, DEC); // แสดงค่า S4
OLED.display(); // สั่งให้ จอแสดงผล
delay(100);
}
}
void loop() {
int sw = digitalRead(button); // ให้ sw อ่านค่า digital จากพอร์ต 2(button)
int nob = analogRead(7); // ให้ nob เทียบเท่าค่า A7
int menu = map(nob, 0, 1023, 0, 9); // เทียบอัตราส่วนของพอร์ต A7 จาก 0-1023 เพื่อทำเป็นเมนู 0-12
OLED.clearDisplay(); // ล้างค่าหน้าจอ
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0
OLED.setTextSize(3); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.print(" "); // วรรค
OLED.println(menu); // แสดงค่า Menu ที่ได้จากการ map nob ให้เหลือ 0-12
OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.println(" ");
OLED.println(" FB:OAT.KT.ROBOT");
OLED.println(" ");
OLED.print(" "); // วรรค
OLED.print(nob); // แสดงค่าที่อ่านได้จาก nob หรือ Analog7
OLED.println(" MeNu");
OLED.display();
int switchs = 1;
if ((sw == switchs) and (menu == 0)) { sensor(); }
if ((sw == switchs) and (menu == 1)) { set_servo(); }
if ((sw == switchs) and (menu == 2)) { menu2(); }
if ((sw == switchs) and (menu == 3)) { menu3(); }
if ((sw == switchs) and (menu == 4)) { menu4(); }
if ((sw == switchs) and (menu == 5)) { menu5(); }
if ((sw == switchs) and (menu == 6)) { menu6(); }
if ((sw == switchs) and (menu == 7)) { menu7(); }
if ((sw == switchs) and (menu == 8)) { menu8(); }
delay(100);
}
//////////////////////////////////////////////////////////////////
void T() //ตัวหลักสำหรับแก้ความเร็วบนเส้นทางท้งหมด
{
analogs();
if ((S0 > cen0) and (S2 > cen2)) { run(80, 80); }
if (S0 < cen0) { run(100, -100); }
if (S2 < cen2) { run(-100, 100); }
}
////////////////////////////////////////////////////////////////////
void B() //ตัวหลักสำหรับแก้ความเร็วบนเส้นทางท้งหมด
{
while (1) {
analogs();
if ((S3 > cen3) and (S4 > cen4)) { run(-50, -50); }
if (S3 < cen3) { run(100, -100); }
if (S4 < cen4) { run(-100, 100); }
if ((S3 < cen3) and (S4 < cen4)) {
run(0, 0);
break;
}
}
}
void PT() //pass เส้นT ==ถ้าเซนเซอร์ทั้ง 2 ตัวเจอดำ หยุด==
{
while (1) {
T();
if (S1 < cen1) {
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 ==ถ้าเซนเซอร์ทั้ง 3 ตัวเจอดำ หยุด เพื่อที่จะทิ้งถุงยังชีพ==
{
while (1) {
T();
if (S1 < cen1) {
run(0, 0);
delay(100);
break;
}
}
}
void PLT() //passleave ==ถ้าเซนเซอร์ทั้ง 3 ตัวเจอดำ หยุด เพื่อที่จะทิ้งถุงยังชีพ==
{
while (1) {
T();
if ((S0 > cen0) and (S2 > cen2)) { run(50, 50); }
if (S0 < cen1) { run(-80, 80); }
if (S2 < cen2) { run(80, -80); }
if ((S0 < cen0) and (S2 < cen2)) {
run(0, 0);
break;
}
}
}
void leave() {
OLED.clearDisplay(); // เคลียร์หน้าจอ oled
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0
OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.println("leave out"); // นำค่า nob มาแสดงใน oled
OLED.display(); // เปิดฟังก์ชันแสดงผล
sv1.write(v_before);
sv2.write(v_before);
sv3.write(v_before);
delay(500);
OLED.clearDisplay(); // เคลียร์หน้าจอ oled
OLED.println("keep int"); // นำค่า nob มาแสดงใน oled
OLED.display(); // เปิดฟังก์ชันแสดงผล
sv1.write(v_after);
sv2.write(v_after);
sv3.write(v_after);
delay(500);
}
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);
}
/////////////////////////////////////////////////////////////////////
/* **MENU** */
void menu2() /// code 2 ที่นี่
{
ST(1000);
PT();
RD(750);
B();
PT();
RD(750);
B();
PT();
RD(750);
B();
PT();
LD(750);
B();
PL();
leave();
}
void menu3() /// code 3 ที่นี่
{
ST(800);
PT();
RD(300);
PT();
RD(300);
PT();
RD(300);
PT();
LD(300);
PD(1500);
LD(300);
PT();
LD(300);
B();
PD(3000);
RD(300);
PT();
LD(300);
PT();
LD(300);
PL();
leave();
B();
LD(300);
PL();
leave();
RD(300);
B();
RD(300);
PD(1000);
LD(300);
PT();
LD(300);
PD(1000);
RD(300);
PT();
RD(300);
PT();
RD(300);
PT();
LD(300);
PT();
LD(300);
PT();
LD(300);
PL();
ST(500);
}
void menu4() /// code 4 ที่นี่
{
B();
}
void menu5() /// code 5 ที่นี่
{
PD(2000);
}
void menu6() /// code 6 ที่นี่
{
ST(1000);
PT();
RD(1000);
PT();
RD(1000);
PT();
RD(1000);
LD(1000);
run(0, 0);
Fall(1000);
RD(1000);
}
void menu7() /// code 7 ที่นี่
{
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 menu8() /// code 7 ที่นี่
{
leave();
}
/*
PT();
PD(เวลา);
PL();
B();
leave();
ST(เวลา);
LD(เวลา);
RD(เวลา);
Fall(เวลา);
*/
///////////////////////////////////////////////////////////////////////
void run(int spl, int spr) // ประกาศฟังก์ชัน run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);
{
if (spl > 0) {
digitalWrite(DL1, LOW);
digitalWrite(DL2, HIGH);
analogWrite(PWML, spl);
} else if (spl < 0) {
spl = abs(spl);
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) {
spr = abs(spr);
digitalWrite(DR1, HIGH);
digitalWrite(DR2, LOW);
analogWrite(PWMR, spr);
} else {
digitalWrite(DR1, HIGH);
digitalWrite(DR2, HIGH);
}
}
1. เมนู 0 วัดความเข้มแสง
2. เมนู 1 sv_knob(); วัดองศาเซอร์โว
3.คำสั่งเซอร์โวยก v_before
4.คำสั่งเซอร์โววาง v_after
5. คำสั่งแทกเส้นหยุดตรงเส้นแยก PT();
6. คำสั่งแทกเส้นตามเวลา PD(เวลา);
7. คำสั่งแทกเส้นพื้นเขียวหยุด PD(เวลา);
8.คำสั่งเลี้ยวขวา RD();
9. คำสั่งเลี้ยวซ้าย LD);
10. คำสั่งถอยหลังไม่ใช้เซนเซอร์ BD();
11. คำสั่งถอยหลังใช้เซนเซอร์B();
12. เจอเขียวแล้วหยุดเพื่อปล่อยกล่องPL();
13. ปล่อยกล่องleave();