บอร์ด Lotus Mega Bot มีคุณสมบัติดังต่อไปนี้
1. พอร์ต Analog เชื่อมต่อเพิ่มเติมจำนวน 15 พอร์ต คือ A0-A14
2. พอรต์ Digital เชื่อมต่อเพิ่มเติมจำนวน 20 พอร์ต คือ D32-D51
3. มีปุ่มกด Digital ที่บอร์ด 1 จุด คือ D53
4. จอ OLED I2C
5. ลำโพงบ้ซเซอร์ D52
6. มีตัวต้านทานปรับค่าได้(K-Nob)ต่อที่พอร์ต Analog 15
7. สามารถใช้กับแบตเตอรี่ที่มีไฟเลี้ยงตั้งแต่ 7.4 V - 9 V(แนะนำ)
8. ใช้กับมอเตอร์กระแสตรงความต่างศักย์ระหว่าง 5-12V กระแสปกติไม่เกิน 1.2 mAh
9. มีพอร์ตสำหรับต่อพ่วงเซอร์โวมอเตอร์ จำนวน 8 พอร์ต ซึ่งใช้ Step down แยกไฟออกจากบอร์ดหลักเพื่อป้องกันบอร์ด Reset กรณ๊ที่ใช้กระแสเกินพิกัด
การจัดเรียงขาสัญญาณของบอร์ด Lotus Maga Bot ทางผู้จัดทำได้ทำการวางเรียงขาการใช้งานดังภาพข้างล่าง
ตัวอย่างโค้ด การตรวจสอบพอร์ตและโมดูลต่างๆในบอร์ด
Arduino IDE Code
//////// ผนวกไลบรารี่ ////////////
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <MechaQMC5883.h>
MechaQMC5883 qmc;
Adafruit_SSD1306 OLED(-1);
#include <Servo.h> // ผนวกไลบรารี่
/////////////////////////////////
///////////ตั้งชื่อพอร์ตต่างๆ///////////////////
int button = 53; /// กำหนดปุ่มกดสวิตซ์ขา 53
int snd = 52; // ให้ snd คือพอร์ต 52 ใช้สำหรับฟังก์ชันเสียง
int vr = 15; /// analog vr port 15
///////////////////////////////////////////////
int s0 ;
int s1 ;
int s2 ;
int s3 ;
int s4 ;
int s5 ;
int s6 ;
int s7 ;
////////////////ตั้งค่าพอร์ตเซอร์โว//////////////////////////
int ss1 = 24; // เปิดใช้งาน s1 ที่พอร์ต 24
int ss2 = 25; // เปิดใช้งาน s1 ที่พอร์ต 25
int ss3 = 26; // เปิดใช้งาน s1 ที่พอร์ต 26
int ss4 = 27; // เปิดใช้งาน s1 ที่พอร์ต 27
int ss5 = 28; // เปิดใช้งาน s1 ที่พอร์ต 28
int ss6 = 29; // เปิดใช้งาน s1 ที่พอร์ต 29
int ss7 = 30; // เปิดใช้งาน s1 ที่พอร์ต 30
int ss8 = 31; // เปิดใช้งาน s1 ที่พอร์ต 31
Servo sv1; // ประกาศการใช้งาน sv1
Servo sv2; // ประกาศการใช้งาน sv2
Servo sv3; // ประกาศการใช้งาน sv3
Servo sv4; // ประกาศการใช้งาน sv4
Servo sv5; // ประกาศการใช้งาน sv5
Servo sv6; // ประกาศการใช้งาน sv6
Servo sv7; // ประกาศการใช้งาน sv7
Servo sv8; // ประกาศการใช้งาน sv8
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////
#define DL1 9 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 9
#define DL2 8 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 8
#define PWML 2 /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 2
//////////////////////////////////
#define DR1 11 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 11
#define DR2 10 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 10
#define PWMR 3 /// กำหนดสัญญาณ PWM ขวาพอร์ต 3
/////////////////////////////////////////////////
/////// ช่อง B //////////////////////////////////
#define DL1b 13 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 9
#define DL2b 12 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 8
#define PWMLb 4 /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 2
//////////////////////////////////
#define DR1b 15 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 11
#define DR2b 14 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 10
#define PWMRb 5 /// กำหนดสัญญาณ PWM ขวาพอร์ต 3
/////// ช่อง c //////////////////////////////////
#define DL1c 17 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 9
#define DL2c 16 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 8
#define PWMLc 6 /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 2
//////////////////////////////////
#define DR1c 23 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 11
#define DR2c 22 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 10
#define PWMRc 7 /// กำหนดสัญญาณ PWM ขวาพอร์ต 3
///////////////////////////////////////////////////////////////////////
/////////////////ตั้งค่าพอร์ต Analog ด้านหน้า//////////////
#define A0 600 // ค่ากลางเซนเซอร์ A0 (ซ้ายสุด)
#define A1 600 // ค่ากลางเซนเซอร์ A1 (ซ้ายกลาง)
#define A2 600 // ค่ากลางเซนเซอร์ A2 (ขวากลาง)
#define A3 600 // ค่ากลางเซนเซอร์ A3 (ขวาสุด)
///////////////////ตั้งค่าพอร์ต Analog ด้านหลัง////////////////////////////////
#define A4 600 // ค่ากลางเซนเซอร์ A0 (หลังซ้ายสุด)
#define A5 600// ค่ากลางเซนเซอร์ A1 (หลังซ้ายกลาง)
#define A6 600 // ค่ากลางเซนเซอร์ A2 (หลังขวากลาง)
#define A7 600 // ค่ากลางเซนเซอร์ A3 (หลังขวาสุด)
//////////////////////////////////////////////////////////
void analogs()
{
s0 = analogRead(0);
s1 = analogRead(1);
s2 = analogRead(2);
s3 = analogRead(3);
s4 = analogRead(4);
s5 = analogRead(5);
s6 = analogRead(6);
s7 = analogRead(7);
}
void setup() {
Serial.begin(9600);
pinMode(2, INPUT); // ตั้งค่าขา 2 เป็น INPUT
pinMode(32,INPUT); pinMode(33,INPUT); pinMode(34,INPUT); pinMode(35,INPUT); pinMode(36,INPUT); pinMode(37,INPUT); pinMode(38,INPUT);
pinMode(39,INPUT); pinMode(40,INPUT); pinMode(41,INPUT); pinMode(42,INPUT); pinMode(43,INPUT); pinMode(44,INPUT); pinMode(45,INPUT);
pinMode(46,INPUT); pinMode(47,INPUT); pinMode(48,INPUT); pinMode(49,INPUT); pinMode(50,INPUT); pinMode(51,INPUT); pinMode(53,INPUT);
tone(snd,900,50);delay(150);tone(snd,900,50);
////////กำหนดสัญญาณทุกขาเป็น OUTPUT ///////////////
pinMode(DL1, OUTPUT);
pinMode(DL2, OUTPUT);
pinMode(PWML, OUTPUT);
pinMode(DR1, OUTPUT);
pinMode(DR2, OUTPUT);
pinMode(PWMR, OUTPUT);
////////////////////////////
pinMode(DL1b, OUTPUT);
pinMode(DL2b, OUTPUT);
pinMode(PWMLb, OUTPUT);
pinMode(DR1b, OUTPUT);
pinMode(DR2b, OUTPUT);
pinMode(PWMRb, OUTPUT);
//////////////////////
pinMode(DL1c, OUTPUT);
pinMode(DL2c, OUTPUT);
pinMode(PWMLc, OUTPUT);
pinMode(DR1c, OUTPUT);
pinMode(DR2c, OUTPUT);
pinMode(PWMRc, OUTPUT);
////////////////////////////
OLED.begin(SSD1306_SWITCHCAPVCC,0x3C); // กำหนดแอดเดรสของพอร์ตจอเป็น 0x3C (for the 128x64)
pinMode(button, INPUT); // ตั้งค่าขา button เป็น INPUT
sv1.attach(ss1); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv1
sv2.attach(ss2); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv2
sv3.attach(ss3); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv3
sv4.attach(ss4); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv4
sv5.attach(ss5); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv5
sv6.attach(ss6); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv6
sv7.attach(ss7); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv7
sv8.attach(ss8); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv8
qmc.init();
sv_set();
}
void loop() {
int sw = digitalRead(button); // ให้ sw อ่านค่า digital จากพอร์ต 2(button)
int nob = analogRead(vr); // ให้ nob เทียบเท่าค่า A7
int menu = map(nob,0,1023,0,12); // เทียบอัตราส่วนของพอร์ต A7 จาก 0-1023 เพื่อทำเป็นเมนู 0-12
OLED.clearDisplay(); // ล้างค่าหน้าจอ
OLED.setTextColor(WHITE,BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
OLED.setCursor(0,0); // เซตตำแหน่ง 0,0
OLED.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.print(" "); // วรรค
OLED.println(menu); // แสดงค่า Menu ที่ได้จากการ map nob ให้เหลือ 0-12
OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.println(" LOTUS MEGA BOT"); // พิมพ์คำว่า KLS ROBOT CENTER
OLED.print(" "); // วรรค
OLED.println("LOTUS ARDUIBOT"); // พิมพ์คำว่า www.kls.ac.th
OLED.print(" ");
OLED.println(nob); // แสดงค่าที่อ่านได้จาก nob หรือ Analog7
OLED.display();
//////////////////////////////////////////////////////
if (menu == 0){OLED.setTextSize(2);OLED.println(" Analog Ck"); OLED.display();}
if (menu == 1){OLED.setTextSize(2);OLED.println(" Servo Ck"); OLED.display();}
if (menu == 2){OLED.setTextSize(2);OLED.println("Digital Ck"); OLED.display();}
if (menu == 3){OLED.setTextSize(2);OLED.println("Compass Ck"); OLED.display();}
if (menu == 4){OLED.setTextSize(2);OLED.println(" Motor Ck"); OLED.display();}
/////////////////////////////////////////////////////
if ((sw == 0) and (menu == 0))
{
beep(); sensor ();
}
if ((sw == 0) and (menu == 1))
{
beep(); sv_knob();
}
if ((sw == 0) and (menu == 2))
{
beep(); menu2();
}
if ((sw == 0) and (menu == 3))
{
beep(); menu3();
}
if ((sw == 0) and (menu == 4))
{
beep(); menu4();
}
if ((sw == 0) and (menu == 5))
{
beep();menu5();
}
if ((sw == 0) and (menu == 6))
{
beep();menu6();
}
if ((sw == 0) and (menu == 7))
{
menu7();beep();
}
if ((sw == 0) and (menu == 8))
{
beep();menu8();
}
if ((sw == 0) and (menu == 9))
{
beep();menu9();
}
if ((sw == 0) and (menu == 10))
{
beep();menu10();
}
if ((sw == 0) and (menu == 11))
{
beep();menu11();
}
if ((sw == 0) and (menu == 12))
{
beep();menu12();
}
delay(100);
}
void sensor (){
while (1) {
/////////// ตั้งค่าเซนเซอร์ตามการอ่านพอร์ตอนาล็อก S0,S1,S2,S3
int s0 = analogRead(0);int s1 = analogRead(1);int s2 = analogRead(2);int s3 = analogRead(3);
int s4 = analogRead(4);int s5 = analogRead(5);int s6 = analogRead(6);int s7 = analogRead(7);
int s8 = analogRead(8);int s9 = analogRead(9);int s10 = analogRead(10);int s11 = analogRead(11);
int s12 = analogRead(12);int s13 = analogRead(13);int s14 = analogRead(14);int s15 = analogRead(15);
////////////////////////////////////////////////////////
OLED.clearDisplay();
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
OLED.setCursor(0,0); // เซตตำแหน่ง 0,0
// OLED.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2
// OLED.println("ROBOT CLUB"); // แสดงค่าข้อความ ROBOT CLUB
OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 1
OLED.print(" A0="); OLED.print(s0); // แสดงค่าเซนเซอร์ S0
OLED.print(" A1="); OLED.println(s1); // แสดงค่าเซนเซอร์ S1
OLED.print(" A2="); OLED.print(s2); // แสดงค่าเซนเซอร์ S2
OLED.print(" A3="); OLED.println(s3); // แสดงค่าเซนเซอร์ S3
OLED.print(" A4="); OLED.print(s4); // แสดงค่าเซนเซอร์ S4
OLED.print(" A5="); OLED.println(s5); // แสดงค่าเซนเซอร์ S5
OLED.print(" A6="); OLED.print(s6); // แสดงค่าเซนเซอร์ S6
OLED.print(" A7="); OLED.println(s7); // แสดงค่าเซนเซอร์ S7
OLED.print(" A8="); OLED.print(s8); // แสดงค่าเซนเซอร์ S0
OLED.print(" A9="); OLED.println(s9); // แสดงค่าเซนเซอร์ S1
OLED.print(" A10="); OLED.print(s10); // แสดงค่าเซนเซอร์ S2
OLED.print(" A11="); OLED.println(s11); // แสดงค่าเซนเซอร์ S3
OLED.print(" A12="); OLED.print(s12); // แสดงค่าเซนเซอร์ S4
OLED.print(" A13="); OLED.println(s13); // แสดงค่าเซนเซอร์ S5
OLED.print(" A14="); OLED.print(s14); // แสดงค่าเซนเซอร์ S6
OLED.print(" A15="); OLED.print(s15); // แสดงค่าเซนเซอร์ S7
OLED.display();
delay(100); // เปลี่ยนค่าเซนเซอร์ทุก 0.1 วินาที
}}
void digi (){
while (1) {
int D32 = digitalRead(32);int D33 = digitalRead(33);int D34 = digitalRead(34);int D35 = digitalRead(35);int D36 = digitalRead(36);
int D37 = digitalRead(37);int D38 = digitalRead(38);int D39 = digitalRead(39);int D40 = digitalRead(40);int D41 = digitalRead(41);
int D42 = digitalRead(42);int D43 = digitalRead(43);int D44 = digitalRead(44);int D45 = digitalRead(45);int D46 = digitalRead(46);
int D47 = digitalRead(47);int D48 = digitalRead(48);int D49 = digitalRead(49);int D50 = digitalRead(50);int D51 = digitalRead(51);
int D53 = digitalRead(53);
OLED.clearDisplay();
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
OLED.setCursor(0,0); // เซตตำแหน่ง 0,0
OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 1
OLED.print(" 32=");OLED.print(D32);OLED.print(" 33=");OLED.print(D33);OLED.print(" 34=");OLED.print(D34);OLED.print(" 35=");OLED.println(D35);
OLED.print(" 36=");OLED.print(D36);OLED.print(" 37=");OLED.print(D37);OLED.print(" 38=");OLED.print(D38);OLED.print(" 39=");OLED.println(D39);
OLED.print(" 40=");OLED.print(D40);OLED.print(" 41=");OLED.print(D41);OLED.print(" 42=");OLED.print(D42);OLED.print(" 43=");OLED.println(D43);
OLED.print(" 44=");OLED.print(D44);OLED.print(" 45=");OLED.print(D45);OLED.print(" 44=");OLED.print(D44);OLED.print(" 47=");OLED.println(D47);
OLED.print(" 48=");OLED.print(D48);OLED.print(" 49=");OLED.print(D49);OLED.print(" 50=");OLED.print(D50);OLED.print(" 51=");OLED.println(D51);
OLED.print(" 53=");OLED.print(D53);
OLED.display();
delay(100); // เปลี่ยนค่าเซนเซอร์ทุก 0.1 วินาที
}}
////////////////////////////////////////////////////////
void sv_knob() {
while(true){
int vr = analogRead(15); // กำหนดตัวแปรจำนวนเต็มอ่านค่าอนาล็อกที่พอร์ต 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.print("SV1-8="); // พิมพ์คำว่า SV1 =
OLED.println(nob); // นำค่า nob มาแสดงใน oled
OLED.display(); // เปิดฟังก์ชันแสดงผล
sv1.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
sv2.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
sv3.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
sv4.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
sv5.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
sv6.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
sv7.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
sv8.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
// delay(50); // หน่วงเวลา 0.05 วินาที
}}
////////// เข็มทิศ ////////////////////////////////
void compass()
{
while (true) {
int x, y, z;
int azimuth;
qmc.read(&x, &y, &z, &azimuth);
analogs();
OLED.clearDisplay();
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0
OLED.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 1
OLED.println(" ");
OLED.print(" Azi = "); OLED.println(azimuth);
OLED.display();
delay(50);
}
}
///////////////////////////////////////////////////////////////////
void RUN(int ml , int mr){runa(ml,mr);runb(ml,mr);runc(ml,mr); } // คำสั่งควบคุมมอเตอร์ทั้ง 6 ช่อง
///////////////////////////////////////////////////////////////////
void run(int spr, int spl) // ประกาศฟังก์ชัน 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);
analogWrite(PWML, -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, LOW);
digitalWrite(DR2, LOW);
analogWrite(PWMR, -255);
}}
void runa(int spra, int spla) // ประกาศฟังก์ชัน run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);
{ if (spla > 0)
{
digitalWrite(DL1, LOW);
digitalWrite(DL2, HIGH);
analogWrite(PWML, spla);
}
else if (spla < 0)
{
digitalWrite(DL1, HIGH);
digitalWrite(DL2, LOW);
analogWrite(PWML, -spla);
}
else
{
digitalWrite(DL1, LOW);
digitalWrite(DL2, LOW);
analogWrite(PWML, -255);
}
///////////////////////////////////////////////
if (spra > 0)
{
digitalWrite(DR1, LOW);
digitalWrite(DR2, HIGH);
analogWrite(PWMR, spra);
}
else if (spra < 0)
{
digitalWrite(DR1, HIGH);
digitalWrite(DR2, LOW);
analogWrite(PWMR, -spra);
}
else
{
digitalWrite(DR1, LOW);
digitalWrite(DR2, LOW);
analogWrite(PWMR, -255);
}}
/////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////
void runb(int sprb, int splb) // ประกาศฟังก์ชัน run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);
{ if (splb > 0)
{
digitalWrite(DL1b, LOW);
digitalWrite(DL2b, HIGH);
analogWrite(PWMLb, splb);
}
else if (splb < 0)
{
digitalWrite(DL1b, HIGH);
digitalWrite(DL2b, LOW);
analogWrite(PWMLb, -splb);
}
else
{
digitalWrite(DL1b, LOW);
digitalWrite(DL2b, LOW);
analogWrite(PWMLb, -255);
}
//////////////////////////////////////
if (sprb > 0)
{
digitalWrite(DR1b, LOW);
digitalWrite(DR2b, HIGH);
analogWrite(PWMRb, sprb);
}
else if (sprb < 0)
{
digitalWrite(DR1b, HIGH);
digitalWrite(DR2b, LOW);
analogWrite(PWMRb, -sprb);
}
else
{
digitalWrite(DR1b, LOW);
digitalWrite(DR2b, LOW);
analogWrite(PWMRb, -255);
}}
//////////////////////////////////////
void runc(int sprc, int splc) // ประกาศฟังก์ชัน run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);
{ if (splc > 0)
{
digitalWrite(DL1c, LOW);
digitalWrite(DL2c, HIGH);
analogWrite(PWMLc, splc);
}
else if (splc < 0)
{
digitalWrite(DL1c, HIGH);
digitalWrite(DL2c, LOW);
analogWrite(PWMLc, -splc);
}
else
{
digitalWrite(DL1c, LOW);
digitalWrite(DL2c, LOW);
analogWrite(PWMLc, -255);
}
//////////////////////////////////////
if (sprc > 0)
{
digitalWrite(DR1c, LOW);
digitalWrite(DR2c, HIGH);
analogWrite(PWMRc, sprc);
}
else if (sprc < 0)
{
digitalWrite(DR1c, HIGH);
digitalWrite(DR2c, LOW);
analogWrite(PWMRc, -sprc);
}
else
{
digitalWrite(DR1c, LOW);
digitalWrite(DR2c, LOW);
analogWrite(PWMRc, -255);
}}
void sv_set() // เซตเซอร์โว
{ sv1.write(67);delay(1000);
sv2.write(35);delay(1000);
}
///////////////////////////////////////////////////////////////////////////////
void beep(){tone(snd, 1200, 50);}
/////////////////////////////////////////////
void motor(){
OLED.clearDisplay();OLED.setCursor(0,0);OLED.setTextSize(2);OLED.println("ALL MOTOR ");OLED.display();
RUN(100,100);delay(2000);RUN(0,0);beep();delay(1000);
RUN(-100,100);delay(2000);RUN(0,0);beep();delay(1000);
RUN(100,-100);delay(2000);RUN(0,0);beep();delay(1000);
RUN(-100,-100);delay(2000);RUN(0,0);beep();delay(1000);
OLED.clearDisplay();OLED.setCursor(0,0);OLED.setTextSize(2);OLED.println(" MOTOR A ");OLED.display();
runa(100,100);delay(2000);runa(0,0);beep();delay(1000);
runa(-100,100);delay(2000);runa(0,0);beep();delay(1000);
runa(100,-100);delay(2000);runa(0,0);beep();delay(1000);
runa(-100,-100);delay(2000);runa(0,0);beep();delay(1000);
OLED.clearDisplay();OLED.setCursor(0,0);OLED.setTextSize(2);OLED.println(" MOTOR B ");OLED.display();
runb(100,100);delay(2000);runb(0,0);beep();delay(1000);
runb(-100,100);delay(2000);runb(0,0);beep();delay(1000);
runb(100,-100);delay(2000);runb(0,0);beep();delay(1000);
runb(-100,-100);delay(2000);runb(0,0);beep();delay(1000);
OLED.clearDisplay();OLED.setCursor(0,0);OLED.setTextSize(2);OLED.println(" MOTOR C ");OLED.display();
runc(100,100);delay(2000);runc(0,0);beep();delay(1000);
runc(-100,100);delay(2000);runc(0,0);beep();delay(1000);
runc(100,-100);delay(2000);runc(0,0);beep();delay(1000);
runc(-100,-100);delay(2000);runc(0,0);beep();delay(1000);
}
/////////////////////////////////////////////
void P()
{ while (1) {
int s0 = analogRead(0); int s1 = analogRead(1); int s2 = analogRead(2); int s3 = analogRead(3);
if (((s1 > A1) and (s2 > A2))) {run(100, 100);}
if (s0 < A0) {run(20, 100);}
if (s1 < A1) {run(30, 100);}
if (s2 < A2) {run(100, 30);}
if (s3 < A3) {run(100, 20);}
if (((s0 < A0) and (s1 < A1)) or ((s1 < A1) and (s2 < A2)) or ((s2 < A2) and (s3 < A3)))
{ run(100, 100); delay(120); run(0, 0); delay(80);
beep(); break;
}
}
}
void B()
{ while (1) {
int s4 = analogRead(4); int s5 = analogRead(5); int s6 = analogRead(6); int s7 = analogRead(7);
if (((s5 > A5) and (s6 > A6))) {run(-100, -100);}
if (s4 < A4) {run(-10, -100);}
if (s5 < A5) {run(-30, -100);}
if (s6 < A6) {run(-100, -30);}
if (s7 < A7) {run(-100, -10);}
if (((s4 < A4) and (s5 < A5)) or ((s5 < A5) and (s6 < A6)) or ((s6 < A6) and (s7 < A7)))
{ run(-100, -100); delay(120); run(0, 0); delay(80);
beep(); break;
}
}
}
/////////////////////////เลี้ยวซ้าย///////////////////////////////////////////
void L0(){while (1){analogs();run(-100,100);if(s0<A0){run(0,0);break;}}} //S0
void L1(){while (1){analogs();run(-100,100);if(s1<A1){run(0,0);break;}}} //S1
void L2(){while (1){analogs();run(-100,100);if(s2<A2){run(0,0);break;}}} //S2
void L3(){while (1){analogs();run(-100,100);if(s3<A3){run(0,0);break;}}} //S3
//////////////////////เลี้ยวขวา//////////////////////////////////////////
void R0(){while (1){analogs();run(100,-100);if(s0<A0){run(0,0);break;}}} //S0
void R1(){while (1){analogs();run(100,-100);if(s1<A1){run(0,0);break;}}} //S1
void R2(){while (1){analogs();run(100,-100);if(s2<A2){run(0,0);break;}}} //S2
void R3(){while (1){analogs();run(100,-100);if(s3<A3){run(0,0);break;}}} //S3
//////////////////////////////////////////////////////////////////////////////////
void L(){L0(); L1();delay(80);beep();}
void R(){R3(); R2();delay(80);beep();}
/////////////////////////////////////////////////////////////////////////////////////
void menu2() /// code 2 ที่นี่
{
digi ();
}
void menu3() /// code 3 ที่นี่
{
compass();
}
void menu4() /// code 4 ที่นี่
{
motor();
}
void menu5() /// code 5 ที่นี่
{
run(-255,255);
}
void menu6() /// code 6 ที่นี่
{
}
void menu7() /// code 7 ที่นี่
{
}
void menu8() /// code 8 ที่นี่
{
}
void menu9() /// code 9 ที่นี่
{
}
void menu10() /// code 10 ที่นี่
{
}
void menu11() /// code 11 ที่นี่
{
}
void menu12() /// code 12 ที่นี่
{
}