Lab 17 การแทร็กพื้นขาวขอบดำพร้อมเปิดการใช้งานร่วมกับเข็มทิศ

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

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

{

}