#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 8 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 8
#define DR2 7 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 7
#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 cen0 = 300;
int cen1 = 300;
///////////////////////องศา เซอร์โว///////////////////////
const int v_before = 150 ; //กำหนดค่ายกมือขึ้น servo 0
const int v_after = 80; //กำหนดค่ายกมือลง 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
}
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
}
void sensor()
{
while (true) {
analogs();
OLED.clearDisplay(); //ล้างหน้าจอ
OLED.setTextColor(WHITE,BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ จอที่ใช้ไม่สามารถเปลี่ยนสีได้
OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0
OLED.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.print(" S0 : "); // แสดงข้อความ S0 :
OLED.print(S0,DEC); // แสดงค่า S0
OLED.print(" S1 : "); // แสดงข้อความ S1 :
OLED.println(S1,DEC); // แสดงค่า S1
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(" LOGI 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 (S1 > cen1)) {run(100, 100);}
if (S0 < cen0) {run(100, -100);}
if (S1 < cen1) {run(-100, 100);}}
////////////////////////////////////////////////////////////////////
/*void B() //ตัวหลักสำหรับแก้ความเร็วบนเส้นทางท้งหมด
{while(1){
analogs();
if ((S3 > cen3) and (S4 > cen4)) {run(-100, -100);}
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 (S0 < cen0 and (S1 > cen1)){run(-100,-100); 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 (S0 < cen0 and (S1 > cen1)){run(0,0); delay(100); break;}}}
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 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 ที่นี่
{
PT();
}
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)
{
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);
analogWrite(PWMR, -255);
}
/////////////////////////////////////////////
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);
analogWrite(PWMR, -255);
}
}