ตอนที่ 2 การเคลื่อนที่ในเขาวงกต

จากตอนที่ 1 การอ่าน Barcode มาสู่ตอนที่ 2 การเคลื่อนที่ในเขาวงกตโดยไม่มีเส้นกำกับ จะต้องมีอุปกรณ์อื่นๆ เข้ามาช่วย โดยในงานนี้ให้ใช้ โมดูลเข็มทิศ HMC5983

โมดูลเข็มทิศ HMC5983

โมดูลเข็มทิศดิจิตอล HMC5983 เป็นตัวตรวจจับสนามแม่เหล็กโลก จึงนำมาประยุกต์เป็นตัวตรวจจับทิศทางเพื่อช่วยกำหนดทิศทางการเคลื่อนที่ของหุ่นยนต์ได้  เหมาะกับพื้นที่สนามที่ไม่มีเส้นกำกับ

การติดตั้งโมดูล

จากรูปโครงสร้างทั้งหมดเป็นพลาสติก ไม่เกิดการรบกวนทำงานของเข็มทิศ  และต้องติดตั้งให้สูงเหนือตัวหุ่นยนต์ขึ้นไป

 

การเชื่อมต่อ

โมดูลเข็มทิศ HMC5983 มีการสื่อสารด้วยระบบบัส I2C ประกอบด้วยขา SDA (ข้อรับส่งข้อมูล) SCL (ขาสัญญาณนาฬิกา)  ซึ่งขาทั้งสองมีติดตั้งอยู่ที่บอร์ด IPST-SE เรียบร้อยแล้ว
 

 

ชุดคำสั่งสำหรับการติดต่อกับ HMC5983

เมื่อใช้งาน IPST-SE กับซอฟต์แวร์ Arduino เวอร์ชั่นล่าสุด จะสามารถใช้งานคำสั่งติดต่อกับเข็มทิศ HMC5983 ได้ทันที โดยมีคำสั่งที่ต้องใช้งานกับกิจกรรมนี้เพียงคำสั่งเดียวคือ

compass_read()

โดยฟังก์ชั่นนี้จะคืนค่า ทิศในช่วง 0-359 ออกมา

ตัวอย่างการอ่านค่าแสดงผลที่ glcd  

  setTextSize(2); glcdMode(1);
      glcd(2, 0, "%d  ", compass_read());

 การใช้งานเข็มทิศนั้น ก็เพื่อให้หุ่นยนต์เคลื่อนที่ตรงไปยังทิศทางที่ต้องการ และให้หุ่นยนต์หมุนตัวตรงไปยังทิศทางที่ต้องการ ดังนั้นก่อนการใช้งานโปรแกรม จำเป็นต้องสำรวจทิศ ของสนามก่อน โดยกำหนดเป็นตัวแปรขึ้นมาดังนี้

int N=50,E=122,S=168,W=300; โดยค่าเหล่านี้ ระบุทิศทั้ง 4 ที่เราเคลื่อนที่ โดยค่าจะไม่เป็นเชิงเส้นมากนัก และสำหรับค่าที่เกิดขึ้น อาจจะไม่เท่ากันทุกจุดในสนาม จะต้องมีการปรับจูน บวกลบค่าเพิ่มเติมจากค่าเหล่านี้ด้วย จากนั้น สร้างฟังกํชั่นเพื่อทำภารกิจต่างๆ

สำหรับฟังก์ชั่นที่เกี่ยวข้องกับการใช้งานเข็มทิศ ต้องยกเครดิตให้ ครูนัฎฐิกา หลอดแก้ว แห่งโรงเรียนบางปลาม้า “สูงสุมารผดุงวิทย์” ที่อนุเคราะห์รูปแบบการเขียนโค้ดที่ดูเข้าใจง่าย แต่ทำงานได้อย่างมีประสิทธิภาพ 

ฟังก์ชั่นหมุนตัวไปจนกระทั่งถึงทิศที่ต้องการ

 

void rightUntil(int c) {
  while (compass_read()<c){sr(40);}
}

void leftUntil(int c) {
  while (compass_read()>c){sl(40);}
}

หลักการเขียนโปรแกรมง่ายๆ คือ ใช้ while ตรวจสอบเงื่อนไขการหมุนตัว โดยหมุนไปจนกระทั่งถึงทิศทางที่ต้องการ การหมุนจะต้องมีทั้งการหมุนทางซ้าย (leftUntil()) และหมุนทางขวา (rightUntil() ) เพื่อป้องกันการเคลื่อนที่ผ่านจุด 0 องศา ซึ่งจะทำให้การตรวจสอบทิศผิดพลาด

ตัวอย่าง

ปัจจุบันหุ่นยนต์อยู่ที่ตำแหน่ง 300 องศา(ทิศตะวันตก :W )  จะเคลื่อนที่ไปที่ตำแหน่ง 50 องศา (ทิศเหนือ : N) โดยปกติเราอาจจะเคลื่อนที่ทางขวา ช่วงเวลาสั้นๆ ได้ แต่ เพื่อไม่ให้ผ่านจุด 0 องศา เราจึงต้องใช้วิธีการเคลื่อนที่หมุนตัวทางซ้ายเพื่อไปที่ตำแหน่ง 50  องศาแทนโดยเขียนได้เป็น

leftUntil(50);

ฟังก์ชั่นเดินตรงไปตามทิศทางที่ต้องการ

สำหรับฟังก์ชั่นนี้ จะเดินตรงไปเรื่อย ๆจนกว่าจะชนกำแพงดังนั้นจะต้องติดตั้งสวิตช์ที่ด้านหน้าหุ่นยนต์ด้วยดังรูป  โดยสวิตช์จะติดทั้งด้านหน้าและด้านหลัง เพื่อ ใช้ตรวจสอบทั้งการเดินหน้าและถอยหลัง

จากรูปสวิตช์ด้านหน้าต่อที่ขา 27 และสวิตช์ด้านหลังต่อที่ขา 30

void trackWallCompass(int c){   //แทร็กเข็มทิศ
    while(in(27)){
      if(compass_read()<=c+2&&compass_read()>=c-2){fd(40); }
      else if (compass_read()<c+2){tr(40);}
      else if (compass_read()>c-2){tl(40);}
    }ao();
  }

void trackWallBackCompass(int c) {   
  while (in(30)) {
    if (compass_read()<=c+2&&compass_read()>=c-2){bk(40);}
    else if (compass_read()<c+2){bk2(0,40);}
    else if (compass_read()>c-2){bk2(40,0);}
  } 
  ao();
}

การทำงานของโปรแกรมคือ ทำงานไปเรื่อยๆ จนเจอกำแพง โดยการอ่านค่าทิศทาง ถ้าค่าทิศทางอยู่ในช่วงผิดพลาดไม่เกิน +/- 2 องศาก็ให้หุ่นยนต์เดินตรงไป ถ้าเอียงไปด้านซ้ายก็ให้เลี้ยวซ้าย เอียงไปด้านขวาก็ให้เลี้ยวขวา

ฟังก์ชั่น เดินตรงด้วยเข็มทิศหยุดการเคลื่อนที่ด้วยเวลา

ฟังก์ชั่นนี้ใช้ในกรณีที่เดินตรงไป แล้วหยุดด้วยระยะทางที่กำหนด  เนื่องจากเราไม่มี encoder ช่วยระบุเวลาที่กำหนดได้ จึงจำเป็นต้องใช้ ฟังก์ชั่น millis() ซึ่งเป็นค่าเวลาของระบบเข้ามาช่วย  โดยฟังก์ชั่นนี้จะทำงานอยู่เบื้องหลังและให้ค่าเวลาส่งกลับมาให้เราตลอดเวลา ในหน่วยเป็นมิลลิวินาที ซึ่งนำค่านี้มาใช้จับเวลาการเคลื่อนที่ที่แม่นยำได้

void trackTimeCompass(int c,long t){ //แทร็กเข็มทิศด้วยเวลา
    long m=millis(); 
    while(millis()<(m+t)){
      if(compass_read()<=c+2&&compass_read()>=c-2){fd(40); }
      else if (compass_read()<c+2){tr(40);}
      else if (compass_read()>c-2){tl(40);}
    }ao();
}

void trackTimeBackCompass(int c,long t){ //แทร็กเข็มทิศ หยุดเมื่อหมดเวลา
    long m=millis(); 
    while(millis()<(m+t)){
      if(compass_read()<=c+2&&compass_read()>=c-2){bk(40); }
      else if (compass_read()<c+2){bk2(0,40);}
      else if (compass_read()>c-2){bk2(40,0);}
    }ao();
}

การทำงานของโปรแกรมจะเคลื่อนที่ตรงไปจนกว่าค่าเวลาจะมากกว่าค่าที่ตั้งไว้

 

 

 

จากฟังก์ชั่นทั้งหมดสามารถนำมาเขียนรูปแบบการเคลื่อนที่ในเขาวงกตได้ดังนี้

void MazeRight() {
  trackWallCompass(N);
  bk(30); delay(200); ao();
  rightUntil(W);
  trackWallCompass(W);  // ใข่ใบที่ 1
  bk(30); delay(200); ao();
  leftUntil(N);
  trackWallCompass(N);   
  bk(30); delay(200); ao();
  rightUntil(E);
  trackWallCompass(E);   // ใข่ใบที่ 2
  trackTimeBackCompass(E,2700);
  leftUntil(N+12);
  trackWallCompass(N+12);   // ใข่ใบที่ 3
  trackTimeBackCompass(N+12,900);
  rightUntil(E);
  trackTimeBackCompass(E,2300);
  leftUntil(N+12);
  trackWallCompass(N+12);   // ใข่ใบที่ 4
  trackWallBackCompass(N+12);
  fd(40);delay(200);ao();
  rightUntil(E);
  trackWallBackCompass(E);  
  fd(40);delay(200);ao();
  leftUntil(N);
  trackWallBackCompass(N);
  trackTimeCompass(N,1500);
  rightUntil(E);
  trackTimeCompass(E,2000);
  rightUntil(S+15);
  trackWallCompass(S+15);
  bk(40); delay(250); ao();
  rightUntil(W);
  trackWallCompass(W);  // ใขใบที่ 5
  bk(40); delay(250); ao();
  leftUntil(S+19);
  trackTimeCompass(S+19,3000);
  bk(40); delay(250); ao();
  }

 

void MazeLeft() {
  bk(30); delay(100);
  trackWallCompass(N);
  bk(30); delay(300); ao();
  rightUntil(E);
  trackWallCompass(E);
  bk(30); delay(300); ao();
  leftUntil(N);
  trackWallCompass(N);
  bk(30); delay(250); ao();
  rightUntil(W);
  trackWallCompass(W);
  bk(30); delay(300); ao();
  leftUntil(S);
  trackWallCompass(S);  //  ใข่ใบที่ 1
  trackWallBackCompass(S);
  fd(30); delay(250); ao();
  leftUntil(E);
  trackTimeCompass(E, 1000);
  leftUntil(N);
  trackWallCompass(N);
  bk(30); delay(150); ao();
  rightUntil(W);
  trackWallCompass(W);
  bk(30); delay(100); ao();
  leftUntil(N);
  trackWallCompass(N);   // ใข่ใบที่ 2
  trackWallBackCompass(N);
  fd(30); delay(250); ao();
  rightUntil(E);
  trackWallCompass(E);
  bk(30); delay(100); ao();
  leftUntil(N);
  trackWallCompass(N);
  bk(30); delay(150); ao();
  rightUntil(E);
  trackWallCompass(E);  // ใข่ใบที่ 3
  trackTimeBackCompass(E, 1000);
  rightUntil(S);
  trackWallCompass(S);
  bk(30); delay(150); ao();
  rightUntil(W);
  trackWallCompass(W);  // ใข่ใบที่ 4
  bk(30); delay(1000); ao();
  trackTimeBackCompass(W, 1000);
  leftUntil(S);
  trackWallCompass(S);
  bk(30); delay(1000); ao(); // ใข่ใบที่ 5
  leftUntil(E);
  trackWallCompass(E);
  bk(30); delay(100); ao();
  rightUntil(S);
  trackTimeCompass(S, 3000);
  LLL(); ao();
}