Embedded Files
สำหรับบอร์ด Zack Robot nano shield
การใช้งาน
- แทกเส้นเจอ ดำ-ดำ หยุด----- P();
- เลี้ยวซ้าย ---------------------- L();
- เลี้ยวขวา ---------------------- R();
Arduino IDE Nano
///////// ผนวกไลบรารี่ ////////////
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 OLED(-1);
///////////ตั้งค่าปุ่มกด///////////////////
int button = 2; /// กำหนดปุ่มกดสวิตซ์ขา 2
//////////////////////////////////////////////
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////
#define DR1 7 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 7
#define DR2 8 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 8
#define PWMR 6 /// กำหนดสัญญาณ PWM ขวาพอร์ต 6
/////////////////////////////////
#define DL1 9 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 9
#define DL2 4 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 4
#define PWML 5 /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 5
/////////////////ตั้งค่าพอร์ต Analog//////////////
#define A0 252 // ค่ากลางเซนเซอร์ A0 (ซ้ายสุด)
#define A1 356 // ค่ากลางเซนเซอร์ A1 (ซ้ายกลาง)
#define A2 379 // ค่ากลางเซนเซอร์ A2 (ขวากลาง)
#define A3 223 // ค่ากลางเซนเซอร์ A3 (ขวาสุด)
///////////////////////////////////////////////
int s0 ;
int s1 ;
int s2 ;
int s3 ;
void analogs()
{
s0 = analogRead(0);
s1 = analogRead(1);
s2 = analogRead(2);
s3 = analogRead(3);
}
//////////////////////////////////////////////
void setup() {
OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C); // กำหนดแอดเดรสของพอร์ตจอเป็น 0x3C (for the 128x64)
pinMode(2, INPUT); // ตั้งค่าขา 2 เป็น INPUT
////////กำหนดสัญญาณทุกขาเป็น OUTPUT /////////
pinMode(DL1, OUTPUT);
pinMode(DL2, OUTPUT);
pinMode(PWML, OUTPUT);
pinMode(DR1, OUTPUT);
pinMode(DR2, OUTPUT);
pinMode(PWMR, OUTPUT);
}
void loop() {
int sw = digitalRead(button); // ให้ sw อ่านค่า digital จากพอร์ต 2(button)
int nob = analogRead(7); // ให้ 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(" KLS ROBOT CENTER"); // พิมพ์คำว่า KLS ROBOT CENTER
OLED.print(" "); // วรรค
OLED.print(nob); // แสดงค่าที่อ่านได้จาก nob หรือ Analog7
OLED.println(" www.kls.ac.th"); // พิมพ์คำว่า www.kls.ac.th
OLED.display();
if ((sw == 0) and (menu == 0))
{ sensor();
}
if ((sw == 0) and (menu == 1))
{
menu1();
}
if ((sw == 0) and (menu == 2))
{
menu2();
}
if ((sw == 0) and (menu == 3))
{
menu3();
}
if ((sw == 0) and (menu == 4))
{
menu4();
}
if ((sw == 0) and (menu == 5))
{
menu5();
}
if ((sw == 0) and (menu == 6))
{
menu6();
}
if ((sw == 0) and (menu == 7))
{
menu7();
}
if ((sw == 0) and (menu == 8))
{
menu8();
}
if ((sw == 0) and (menu == 9))
{
menu9();
}
if ((sw == 0) and (menu == 10))
{
menu10();
}
if ((sw == 0) and (menu == 11))
{
menu11();
}
if ((sw == 0) and (menu == 12))
{
menu12();
}
delay(100);
}
void sensor()
{
while (true) {
analogs();
OLED.clearDisplay();
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0
OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 1
OLED.print(" S0 = "); OLED.println(s0); // แสดงค่าเซนเซอร์ S0
OLED.print(" S1 = "); OLED.println(s1); // แสดงค่าเซนเซอร์ S1
OLED.print(" S2 = "); OLED.println(s2); // แสดงค่าเซนเซอร์ S2
OLED.print(" S3 = "); OLED.println(s3); // แสดงค่าเซนเซอร์ S3
OLED.display();
delay(50);
}
}
///////////////////////เจอดำหยุด////////////////////////////////////////////
/////////////////////////เลี้ยวซ้าย///////////////////////////////////////////
void L0(){while (1){analogs();run(-75,75);if(s0<A0){run(0,0);break;}}} //S0
void L1(){while (1){analogs();run(-75,75);if(s1<A1){run(0,0);break;}}} //S1
void L2(){while (1){analogs();run(-75,75);if(s2<A2){run(0,0);break;}}} //S2
void L3(){while (1){analogs();run(-75,75);if(s3<A3){run(0,0);break;}}} //S3
//////////////////////เลี้ยวขวา//////////////////////////////////////////
void R0(){while (1){analogs();run(75,-75);if(s0<A0){run(0,0);break;}}} //S0
void R1(){while (1){analogs();run(75,-75);if(s1<A1){run(0,0);break;}}} //S1
void R2(){while (1){analogs();run(75,-75);if(s2<A2){run(0,0);break;}}} //S2
void R3(){while (1){analogs();run(75,-75);if(s3<A3){run(0,0);break;}}} //S3
/////////////////////////////////////////////////////////////////////
/////////////////////////ฟังก์ชันเส้นทาง////////////////////////////////////////
/////////////////////////ฟังก์ชันเส้นทาง////////////////////////////////////////
void P() ///////////// ฟังก์ชันแทกเส้นเจอคู่ดำหยุด/////////////////////////////
{while (1){
s0 = analogRead(0);
s1 = analogRead(1);
s2 = analogRead(2);
s3 = analogRead(3);
if ((s1 > A1) and (s2 > A2)){run(75, 75);}
if (s1 < A1){run(30, 75);}
if (s2 < A2){run(75, 30);}
if (s0 < A0){run(-40, 75);}
if (s3 < A3){run(75, -40);}
if (((s1 < A1) and (s2 < A2)) or ((s1 < A1) and (s0 < A0)) or ((s2 < A2) and (s3 < A3)))
{run(75, 75);delay(180);run(0, 0);delay(100);tone(3,1000,100);break;}}}
void L(){L3();L1();delay(100);tone(3,1000,50);} /////// เลี้ยวซ้าย
void R(){R0;R2();delay(100);tone(3,1000,50);} ///////// เลี้ยว
//////////////////////////////////////////////////////////////////////////
/////////////////////////////ฟังก์ชันมอเตอร์///////////////////////////////////
/////////////////////////////ฟังก์ชันมอเตอร์///////////////////////////////////
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, LOW);
digitalWrite(DL2, LOW);
}
//////////////////////////////////////
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);
}
}
//////////////////////////////// เรียกใช้ code menu ////////////
void menu1() /// code 1 ที่นี่
{
P();
}
void menu2() /// code 2 ที่นี่
{
L();
}
void menu3() /// code 3 ที่นี่
{
R();
}
void menu4() /// code 4 ที่นี่
{
}
void menu5() /// code 5 ที่นี่
{
}
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 ที่นี่
{
}
สำหรับบอร์ด UNO
การใช้งาน
- แทกเส้นเจอ ดำ-ดำ หยุด----- P();
- เลี้ยวซ้าย ---------------------- L();
- เลี้ยวขวา ---------------------- R();
Arduino UNO
//////////////////ผนวกไลบรารี่จอแสดงผล//////////////////
#include <Wire.h>
#include <SPI.h>
//////////////////////////////////////กำหนดตัวแปร
int s0 ;
int s1 ;
int s2 ;
int s3 ;
///////////ตั้งค่าปุ่มกด///////////////////
int button = 2; /// กำหนดปุ่มกดสวิตซ์ขา 2
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////
#define DR1 7 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 7
#define DR2 8 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 8
#define PWMR 6 /// กำหนดสัญญาณ PWM ขวาพอร์ต 6
/////////////////////////////////
#define DL1 9 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 9
#define DL2 4 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 4
#define PWML 5 /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 5
/////////////////ตั้งค่าพอร์ต Digital//////////////
void digital() {
s0 = digitalRead(10);
s1 = digitalRead(11);
s2 = digitalRead(12);
s3 = digitalRead(13);
}
/////////////////////แท็ก/////////////////////
void track() {
digital();
if ((s1 == 0) and (s2 == 0))
{run(100, 100);}
if (s0 == 1)
{run(-150, 150);
delay(500);}
if (s1 == 1)
{run(-150, 150);
delay(300);}
if (s2 == 1)
{run(150, -150);
delay(300);}
if (s3 == 1)
{run(150, -150);
delay(500);}
}
/////////////////////////////////////////////////////
void P()
{ while (1)
{ int s0 = digitalRead(10);
int s1 = digitalRead(11);
int s2 = digitalRead(12);
int s3 = digitalRead(13);
if ((s1 == 0) and (s2 == 0)){run(100, 100);}
// if (s0 == 1) {run(-150, 150);delay(50);}
if (s1 == 1) {run(-130, 130);delay(30);}
if (s2 == 1) {run(130, -130);delay(30);}
// if (s3 == 1) {run(150, -150);delay(50);}
if (((s1 == 1) and (s2 == 1)) or ((s1==1) and (s0==1)) or ((s2==1) and (s3==1))) {tone(3,1000,100); run(150, 150);delay(300); run(0, 0); delay(300); break;}}}
////////////////////////////////////////////////////2
void setup() {
//Serial.print(s0);
//Serial.print(s1);
//Serial.print(s2);
//Serial.println(s3);
//////////////////////////////////////////////////
pinMode(2, INPUT); // ตั้งค่าขา 2 เป็น INPUT
pinMode(10, INPUT);
pinMode(11, INPUT);
pinMode(12, INPUT);
pinMode(13, INPUT);
////////กำหนดสัญญาณทุกขาเป็น OUTPUT ///////////////
pinMode(DL1, OUTPUT);
pinMode(DL2, OUTPUT);
pinMode(PWML, OUTPUT);
pinMode(DR1, OUTPUT);
pinMode(DR2, OUTPUT);
pinMode(PWMR, OUTPUT);
////////////////////////////
}
/////////////////////////////////////////////////////////////////
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, 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);
}
}
////////////////////เสี้ยวซ้าย S เจอดำหยุด//////////////////////////////////
void L0(){while(1){digital();run(-255,255);if(s0==1){run(0,0);break;}}}
void L1(){while(1){digital();run(-255,255);if(s1==1){run(0,0);break;}}}
void L2(){while(1){digital();run(-255,255);if(s2==1){run(0,0);break;}}}
void L3(){while(1){digital();run(-255,255);if(s3==1){run(0,0);break;}}}
///////////////////เสี้ยวขวา S เจอดำหยุด///////////////////////////////////
void R0(){while(1){digital();run(255,-255);if(s0==1){run(0,0);break;}}}
void R1(){while(1){digital();run(255,-255);if(s1==1){run(0,0);break;}}}
void R2(){while(1){digital();run(255,-255);if(s2==1){run(0,0);break;}}}
void R3(){while(1){digital();run(255,-255);if(s3==1){run(0,0);break;}}}
//////////////////////////////////////////////////////////////////////
void L (){L3();L1();delay(100); run(150,150); delay(100); run(0,0); delay(100); tone(3,1000,50);}
void R (){R0();R2();delay(100); run(150,150); delay(100); run(0,0); delay(100); tone(3,1000,50);}
///////////////////////////////////////////////////////////////////
void loop() {
int sw = digitalRead(button);
if (sw == 1) {
P(); ///// เขียน code ที่นี่
}}
//////////////////////////////////////////////////////////////////
Page updated
Google Sites
Report abuse