ภารกิจหุ่นยนต์ เคลื่อนย้ายกระป๋องจาก จุดที่ 1 ไปยังจุดที่ 2 โดยมีรูปแบบเส้นทางดังรูป

สร้างหุ่นยนต์

ก่อนอื่นสร้างหุ่นยนต์ โดยมีส่วนประกอบคือ

• หุ่นยนต์ IPST-SE มาตรฐาน 2
• ชุดมือจับ SM-Gripper

1. จากรูปแบบเดิมของชุดหุ่นยนต์ IPST ให้ถอดชิ้นส่วนด้านบนและตัวบอร์ด IPSE-SE แยกออกมาก

2. จะได้ชุดฐานลักษณะดังรูป ให้ติดตั้งเสารอง 32 มม. เพื่อเตรียมยึดเข้ากับ SM-Gripper

3. สำหรับ SM-Gripper ให้นำบอร์ด IPST-SE ไปยึดที่ด้านหลังดังรูป

4. ติดตั้ง SM-Gripper บนฐานของ IPST-SE พร้อมทั้งติดตั้งเซนเซอร์ ZX-03 ไว้ที่ด้านหน้าของหุ่นยนต์ 2 ตัว โดยด้านซ้ายต่อเข้ากับช่อ A0 ด้านขวาต่อเข้ากับช่อง A1

5. ติดตั้งแบตเตอรี่ Li-Po ขนาด 7.4V

สร้างฟังก์ชั่น สำหรับภารกิจต่าง ๆ

ฟังก์ชั่น RRR()

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

void RRR(){
  while(1){
    L=analog(0);R=analog(1);
    if (L<650&&R<650){
      fd(40); delay(100);
      while(analog(1)>650){sr(40);}
      while(analog(1)<650){sr(40);}
      break;
    }
    else if(L>650&&R>650){fd(40);}
    else if(L<650&&R>650){sl(40);}
    else if(L>650&&R<650){sr(40);}
  }
}

ฟังก์ชั่น LLL()

ฟังก์ชั่นเคลื่อนที่ตามเส้นเมื่อเจอเส้นตัดให้เลี้ยวซ้ายแล้วออกจากฟังก์ชั่น

void LLL(){
  while(1){
    L=analog(0); R=analog(1);
    if (L<650&&R<650){
      fd(40);delay(100);
      while(analog(0)>650){sl(40);}
      while(analog(0)<650){sl(40);}
      break;
    }
    else if(L>650&&R>650){fd(40);}
    else if(L<650&&R>650){sl(40);}
    else if(L>650&&R<650){sr(40);}
  }
}

ฟังก์ชั่น FFF()

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

void FFF(){
  while(1){
    L=analog(0); R=analog(1);
    if (L<650&&R<650){
      fd(40);delay(100);
      break;
    }
    else if(L>650&&R>650){fd(40);}
    else if(L<650&&R>650){sl(40);}
    else if(L>650&&R<650){sr(40);}
  }
}

ฟังก์ชั่น sKeep()

ฟังก์ชั่นสำหรับสั่งงานเซอร์โวเพื่อเก็บกระป๋องมาไว้ที่ตัว

void sKeep(){
  servo(1,125);delay(300);
  servo(0,130);delay(300);
  servo(1,35);delay(300);
}

ฟังก์ชั่น sPush()

ฟังก์ชั่นสำหรับสั่งงานเซอร์โวเพื่อวางกระป๋อง

void sPush(){
 servo(1,125);delay(300);
 servo(0,30);delay(300);
 servo(1,35);delay(300);
}

ฟังก์ชั่น CAN()

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

void CAN(){
  while(1){
    L=analog(0); R=analog(1);
    if (L<650&&R<650){
      ao();delay(200);
      sKeep();
      bk(40);delay(300);
      while(analog(1)>650){sr(40);}
      while(analog(1)<650){sr(40);}
      break;		
    }
    else if(L>650&&R>650){fd(40);}
    else if(L<650&&R>650){sl(40);}
    else if(L>650&&R<650){sr(40);}	
  }  
}

ฟังก์ชั่น XXX()

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

void XXX(){
  while(1){
    L=analog(0); 
    R=analog(1);
    if (L<650&&R<650){
      ao();delay(200);
      sPush();
      bk(40);delay(300);
      while(analog(1)>650){sr(40);}
      while(analog(1)<650){sr(40);}
      break;		
    }
    else if(L>650&&R>650){fd(40);}
    else if(L<650&&R>650){sl(40);}
    else if(L>650&&R<650){sr(40);}	
  }   
}

จาก รายละเอียดทั้งหมด สามารถนำมาเขียนเป็นโปรแกรมได้ตามนี้ครับ

และผลลัพธ์ที่ได้เป็นดังนี้