3.การใช้งานทั่วไป
Lab 20 Track HMC5883
รูปแบบคำสั่งที่ใช้งาน
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. p(ตัวเลข องศาเข็มทิศ); แทร็กล็อกตำแหน่งที่องศาเข็มทิศตามที่กรอก ** กรณีมีโมดูล HMC5883L
12.B(ตัวเลข); ถอยหลังตามจำนวนนับ
13. sv_set(); ตั้งค่าองศาเซอร์โวเริ่มต้นทำงาน
14. sv_up(); ยิงกระสุนกระบอกชุดที่ 1
15. 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) {
while (1) {
int x, y, z;int azimuth; qmc.read(&x, &y, &z, &azimuth);
if (azimuth >= (cp + 5)) {run(150, -150);int x, y, z;int azimuth; qmc.read(&x, &y, &z, &azimuth);}
else{run(100,120);int x, y, z;int azimuth; qmc.read(&x, &y, &z, &azimuth);}
}}
////////////////////มุมห้าม 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 ที่นี่
{l(30);
}
void menu11() /// code 11 ที่นี่
{p(100);
}
void menu12() /// code 12 ที่นี่
{
}
การกรอกค่าสี