Lab17 การใช้งานหุ่นยนต์แทร็กห้องและการแยกสีแบบเป็นฟังก์ชัน

รูปแบบคำสั่งที่ใช้งาน

1. Menu 0 อ่านค่าแสง Analog, อ่านค่ามุมเข็มทิศ,อ่านค่าสีพื้น น้ำเงิน แดง เขียว เหลือง

2. Menu 1 วัดค่าแสง **กรณีมีโมดูล ADS1115/ADS1015 I2C (เพิ่มจำนวนพอร์ตฝั่งซ้ายของบอร์ด AA0-AA3)

3. Menu 2 ตั้งค่าเซอร์โวตัวที่1

4.S(ตัวเลข); ออกตัวจากจุดเริ่มต้น

5.P(); แทร็กห้องหยุดเมื่อเจอสี/ดำ

6.P(ตัวเลข); แทร็กห้องตามจำนวนนับ

7. R(ตัวเลข); เลี้ยวขวาตามจำนวนนับ

8. L(ตัวเลข); เลี้ยวซ้ายตามจำนวนนับ

9. r(ตัวเลข องศาเข็มทิศ); เลี้ยวขวาแล้วหยุดล็อกตำแหน่งที่องศาเข็มทิศตามที่กรอก ** กรณีมีโมดูล HMC5883L

10. l (ตัวเลข องศาเข็มทิศ); เลี้ยวขวาแล้วหยุดล็อกตำแหน่งที่องศาเข็มทิศตามที่กรอก ** กรณีมีโมดูล HMC5883L

11.B(ตัวเลข); ถอยหลังตามจำนวนนับ

12. sv_set(); ตั้งค่าองศาเซอร์โวเริ่มต้นทำงาน

13. sv_up(); ยิงกระสุนกระบอกชุดที่ 1

14. sv_up2(); ยิงกระสุนกระบอกชุดที่ 2

การติดตั้งตำแหน่ง Sensor Lotus T3

Arduino IDE Code

////// ผนวกไลบรารี่ ////////////

#include <Wire.h>

#include <MechaQMC5883.h>

#include <SPI.h>

#include <Adafruit_ADS1015.h>

#include <Adafruit_GFX.h>

#include <Adafruit_SSD1306.h>

#include <Servo.h>

Adafruit_SSD1306 OLED(-1);

MechaQMC5883 qmc;

Adafruit_ADS1115 ads1115;

///////////ตั้งค่าปุ่มกด///////////////////

int button = 2; /// กำหนดปุ่มกดสวิตซ์ขา 2

int ss1 = 10; // เปิดใช้งาน s1 ที่พอร์ต 10

int ss2 = 11; // เปิดใช้งาน s1 ที่พอร์ต 11

int ss3 = 12; // เปิดใช้งาน s1 ที่พอร์ต 12

Servo sv1; // ประกาศการใช้งาน sv1

Servo sv2; // ประกาศการใช้งาน sv2

Servo sv3; // ประกาศการใช้งาน sv3

//////////////////////////////////////////////

////////////ตั้งค่าพอร์ตมอเตอร์////////////////////

#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 300 // ค่ากลางเซนเซอร์ A0 (ซ้ายสุด) *********

#define A1 300// ค่ากลางเซนเซอร์ A1 (ซ้ายกลาง)

#define A2 300 // ค่ากลางเซนเซอร์ A2 (ขวากลาง)

#define A3 300// ค่ากลางเซนเซอร์ A3 (ขวาสุด) ***********

#define A6 300// ค่ากลางเซนเซอร์ A6 (ตรงกลาง)***********

//////////แสตมป์ค่าเช็คสี RGB /////////////////

//////////แสตมป์ค่าเช็คสี RGB ////////////////

/////////ค่าสี S1R ใช้เซนเซอร์ T3 สีแดง////////

#define Y01 10000 // กรอกค่าที่วัดได้สีเหลือง

#define G01 10000 // กรอกค่าที่วัดได้สีเขียว

#define R01 10000 // กรอกค่าที่วัดได้สีแดง

#define B01 10000 // กรอกค่าที่วัดได้สีน้ำเงิน

//////////////////////////////////////////////

/////////ค่าสี S2B ใช้เซนเซอร์ T3 สีน้ำเงิน/////////////////////

#define Y02 10000 // กรอกค่าที่วัดได้สีเหลือง

#define G02 10000 // กรอกค่าที่วัดได้สีเขียว

#define R02 10000 // กรอกค่าที่วัดได้สีแดง

#define B02 10000 // กรอกค่าที่วัดได้สีน้ำเงิน

//////////////////////////////////////////////

/////////ค่าสี S6G ใช้เซนเซอร์ T3 สีเขียว/////////////////////

#define Y06 10000 // กรอกค่าที่วัดได้สีเหลือง

#define G06 10000 // กรอกค่าที่วัดได้สีเขียว

#define R06 10000 // กรอกค่าที่วัดได้สีแดง

#define B06 10000 // กรอกค่าที่วัดได้สีน้ำเงิน

///////////////////////////////////////////////////////

int s0 ;

int s1 ;

int s2 ;

int s3 ;

int s6 ;

void analogs()

{

s0 = analogRead(0);

s1 = analogRead(1);

s2 = analogRead(2);

s3 = analogRead(3);

s6 = analogRead(6);

}

//////////////////////////////////////////////

void setup() {

OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C); // กำหนดแอดเดรสของพอร์ตจอเป็น 0x3C (for the 128x64)

Wire.begin();

qmc.init();

ads1115.begin(); // เปิดการใช้งาน ADS1115

pinMode(button, INPUT); // ตั้งค่าขา 2 เป็น INPUT

////////กำหนดสัญญาณทุกขาเป็น OUTPUT /////////

pinMode(DL1, OUTPUT);

pinMode(DL2, OUTPUT);

pinMode(PWML, OUTPUT);

pinMode(DR1, OUTPUT);

pinMode(DR2, OUTPUT);

pinMode(PWMR, 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(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 ads(){

while(true){

int16_t adc0, adc1, adc2, adc3;

adc0 = ads1115.readADC_SingleEnded(0);

adc1 = ads1115.readADC_SingleEnded(1);

adc2 = ads1115.readADC_SingleEnded(2);

adc3 = ads1115.readADC_SingleEnded(3);

/////////// ทำการ Map ค่าเซนเซอร์ Analog ที่ได้ค่าเหมาะสมกับเซนเซอร์ Lotus T3 /////

int AA0 = map(adc0,150,25000,0,1023);

int AA1 = map(adc1,150,25000,0,1023);

int AA2 = map(adc2,150,25000,0,1023);

int AA3 = map(adc3,150,25000,0,1023);

//////////////////////////////////////////////////////////////////////////////

OLED.clearDisplay();

OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ

OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0

OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 1

OLED.println(" ANALOG I2C TEST");

OLED.print(" AA0 = "); OLED.println(AA0); /// แสดงผลค่าอะนาล็อกMapที่พอร์ต AA0

OLED.print(" AA1 = "); OLED.println(AA1); /// แสดงผลค่าอะนาล็อกMapที่พอร์ต AA1

OLED.print(" AA2 = "); OLED.println(AA2); /// แสดงผลค่าอะนาล็อกMapที่พอร์ต AA2

OLED.print(" AA3 = "); OLED.println(AA3); /// แสดงผลค่าอะนาล็อกMapที่พอร์ต AA3

OLED.display();

delay(50);

}}

void sensor()

{

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(1); // เซตขนาดอักษรมีขนาดเป็น 1

OLED.print(" S0 = "); OLED.println(s0); // แสดงค่าเซนเซอร์ S0

OLED.print(" S1R = "); OLED.println(s1); // แสดงค่าเซนเซอร์ S1

OLED.print(" S2B = "); OLED.println(s2); // แสดงค่าเซนเซอร์ S2

OLED.print(" S3 = "); OLED.println(s3); // แสดงค่าเซนเซอร์ S3

OLED.print(" S6G = "); OLED.println(s6); // แสดงค่าเซนเซอร์ S3

// OLED.print(" D2 = "); OLED.println(digitalRead(button)); // แสดงค่าเซนเซอร์ S3

OLED.print(" Azi = "); OLED.println(azimuth);

if ( ( s1>=(Y01 - 100) ) and (s1 < (Y01 + 100))and( s2>=(Y02 - 100) ) and (s2 < (Y02 + 100))and( s6>=(Y06 - 70) ) and (s6 < (Y06 + 70)) ){OLED.println(" Yelow");}

if ( ( s1>=(G01 - 100) ) and (s1 < (G01 + 100))and( s2>=(G02 - 100) ) and (s2 < (G02 + 100))and( s6>=(G06 - 70) ) and (s6 < (G06 + 70)) ){OLED.println(" Green");}

if ( ( s1>=(R01 - 100) ) and (s1 < (R01 + 100))and( s2>=(R02 - 100) ) and (s2 < (R02 + 100))and( s6>=(R06 - 70) ) and (s6 < (R06 + 70)) ) {OLED.println(" Red");}

if ( ( s1>=(B01 - 100) ) and (s1 < (B01 + 100))and( s2>=(B02 - 100) ) and (s2 < (B02 + 100))and( s6>=(B06 - 70) ) and (s6 < (B06 + 70)) ) {OLED.println(" Blue");}

else { OLED.println(" -------");}

OLED.display();

delay(50);

}

}

/////////////////////// ฟังก์ชันเซฮร์โว //////////////////////////

void up() {

sv1.write(0); // เซอร์โวยก

delay(1000);

}

void down() {

sv1.write(30); // เซอร์โววาง

delay(1000);

}

/////////////////////////////ฟังก์ชันเซอร์โว////////////////////////////////////////

void sv_knob() {

while (true) {

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.print("SV1 = "); // พิมพ์คำว่า SV1 =

OLED.println(nob); // นำค่า nob มาแสดงใน oled

OLED.display(); // เปิดฟังก์ชันแสดงผล

sv1.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้

delay(50); // หน่วงเวลา 0.05 วินาที

}

}

/////////////////////////////////////////////////////

/////////////////////////////////////////////////////

void sv_set() {sv1.write(180); } // ตั้งค่าเซอร์โวตอนเริ่มต้น

void sv_up() // ตั้งค่าเซอร์โวตัวที่ 1 ยิงแต้ม

{

sv1.write(180);delay(200); // สเต็บที่ 1

sv1.write(130);delay(200); // สเต็บที่ 2

sv1.write(180);delay(200); // สเต็บที่ 3

//sv_set(); // คืนค่าเริ่มต้น

}

void sv_up2() // ตั้งค่าเซอร์โวตัวที่ 1 ยิงแต้ม

{

sv2.write(180);delay(200); // สเต็บที่ 1

sv2.write(130);delay(200); // สเต็บที่ 2

sv2.write(180);delay(200); // สเต็บที่ 3

//sv_set(); // คืนค่าเริ่มต้น

}

///////////////////////////////////////////////////////////////////////

void P() ///////////// ฟังก์ชันแทกเส้นเจอคู่ดำหยุด/////////////////////////////

{

while (1) {

int16_t adc0, adc1, adc2, adc3;

adc0 = ads1115.readADC_SingleEnded(0);

adc1 = ads1115.readADC_SingleEnded(1);

adc2 = ads1115.readADC_SingleEnded(2);

adc3 = ads1115.readADC_SingleEnded(3);

/////////// ทำการ Map ค่าเซนเซอร์ Analog ที่ได้ค่าเหมาะสมกับเซนเซอร์ Lotus T3 /////

int AA0 = map(adc0,150,25000,0,1023);

int AA1 = map(adc1,150,25000,0,1023);

int AA2 = map(adc2,150,25000,0,1023);

int AA3 = map(adc3,150,25000,0,1023);

//////////////////////////////////////////////////////////////////////////

s0 = analogRead(0);

s1 = analogRead(1);

s2 = analogRead(2);

s3 = analogRead(3);

s6 = analogRead(6);

if ((s0 > A0) and (s3 > A3)) {run(60,60);}

if (s0 < A0) {run(70,-70);delay(40);}

if (s3 < A3) {run(-70, 70);delay(40);}

if (s6 < A6) {run(0,0);delay(100);tone(3,1000,100);break; }

if ( ( s1>=(Y01 - 100) ) and (s1 < (Y01 + 100))and( s2>=(Y02 - 100) ) and (s2 < (Y02 + 100))and( s6>=(Y06 - 70) ) and (s6 < (Y06 + 70)) ){OLED.clearDisplay();OLED.setTextColor(WHITE, BLACK);OLED.setCursor(0, 0); OLED.println(" Yelow"); OLED.display();run(0, 0);delay(2000);tone(3, 1000, 100);break;}

if ( ( s1>=(G01 - 100) ) and (s1 < (G01 + 100))and( s2>=(G02 - 100) ) and (s2 < (G02 + 100))and( s6>=(G06 - 70) ) and (s6 < (G06 + 70)) ){OLED.clearDisplay();OLED.setTextColor(WHITE, BLACK);OLED.setCursor(0, 0); OLED.println(" Green"); OLED.display();run(0, 0);delay(2000);tone(3, 1000, 100);break;}

if ( ( s1>=(R01 - 100) ) and (s1 < (R01 + 100))and( s2>=(R02 - 100) ) and (s2 < (R02 + 100))and( s6>=(R06 - 70) ) and (s6 < (R06 + 70)) ) {OLED.clearDisplay();OLED.setTextColor(WHITE, BLACK);OLED.setCursor(0, 0); OLED.println(" Red"); OLED.display();run(0, 0);delay(2000);tone(3, 1000, 100);break;}

if ( ( s1>=(B01 - 100) ) and (s1 < (B01 + 100))and( s2>=(B02 - 100) ) and (s2 < (B02 + 100))and( s6>=(B06 - 70) ) and (s6 < (B06 + 70)) ) {OLED.clearDisplay();OLED.setTextColor(WHITE, BLACK);OLED.setCursor(0, 0); OLED.println(" Blue"); OLED.display();run(0, 0);delay(2000);tone(3, 1000, 100);break;}

}

}

void P(int t) ///////////// ฟังก์ชันแทกเส้น/////////////////////////////

{ int j = 0;

while (1) {

int16_t adc0, adc1, adc2, adc3;

adc0 = ads1115.readADC_SingleEnded(0);

adc1 = ads1115.readADC_SingleEnded(1);

adc2 = ads1115.readADC_SingleEnded(2);

adc3 = ads1115.readADC_SingleEnded(3);

/////////// ทำการ Map ค่าเซนเซอร์ Analog ที่ได้ค่าเหมาะสมกับเซนเซอร์ Lotus T3 /////

int AA0 = map(adc0,150,25000,0,1023);

int AA1 = map(adc1,150,25000,0,1023);

int AA2 = map(adc2,150,25000,0,1023);

int AA3 = map(adc3,150,25000,0,1023);

//////////////////////////////////////////////////////////////////////////

s0 = analogRead(0);

s1 = analogRead(1);

s2 = analogRead(2);

s3 = analogRead(3);

s6 = analogRead(6);

if ((s0 > A0) and (s3 > A3)) {run(60,60);}

if (s0 < A0) {run(70,-70);delay(40);}

if (s3 < A3) {run(-70, 70);delay(40);}

if (j > t) {run(0, 0);tone(3, 900, 500);break;} j++;}}

///////////////////////// แทร็กทิศ //////////////////////////////////////////////////////

void p(int cp) {

int x, y, z;int azimuth;

while (1) {

qmc.read(&x, &y, &z, &azimuth);

if (azimuth > (cp + 5)) {run(150, -50);}

if (azimuth < (cp - 5)) {run(-50, 150);}

else{run(200,200);}

}}

////////////////////มุมห้าม 255-5 องศา//////////////////////////////////////////////////////

void l(int cp) {while (1) {run(-50, 50);int x, y, z;int azimuth;qmc.read(&x, &y, &z, &azimuth);if ((azimuth > (cp - 5)) and (azimuth < (cp + 5))) {run(0, 0);tone(3, 900, 500);break;}}}

void r(int cp) {while (1) {run(50, -50);int x, y, z;int azimuth;qmc.read(&x, &y, &z, &azimuth);if ((azimuth > (cp - 5)) and (azimuth < (cp + 5))) {run(0, 0);tone(3, 900, 500);break; }}}

/////////////////////////////////////////////////////////////////////////////////////////////

void L(int cp) {run(-60, 60);delay(cp);run(0, 0);tone(3, 900, 500);}

void R(int cp) {run(60, -60);delay(cp);run(0, 0);tone(3, 900, 500);}

void B(int cp) {run(-60,-60);delay(cp);run(0, 0);tone(3, 900, 500);}

void S(int cp) {run(60,60);delay(cp);run(0, 0);tone(3, 900, 500);} // ออกตัวสตาร์ท

/////////////////////////////ฟังก์ชันมอเตอร์///////////////////////////////////

/////////////////////////////ฟังก์ชันมอเตอร์///////////////////////////////////

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);

}

}

//////////////////////////////// เรียกใช้ code menu ////////////

void menu1() /// code 1 ที่นี่

{

ads();

}

void menu2() /// code 2 ที่นี่

{

sv_knob();

}

void menu3() /// code 3 ที่นี่

{

}

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 ที่นี่

{

}

ดาวน์โหลดโปรแกรมคำนวณค่าการสะท้อนแสงที่นี่

การกรอกค่าสี