บทที่ 6 IPST-BOT กับการหลบหลีก สิ่งกีดขวางแบบสัมผัส 221 บทที่ 6 IPST-BOT กับการหลบหลีก สิ่งกีดขวางแบบสัมผัส

การทดลองที่ 3 (ตรวจจับการชนด้วยสวิตซ์ 1 ตัว) 222 การทดลองที่ 3 (ตรวจจับการชนด้วยสวิตซ์ 1 ตัว) การเชื่อมต่อทางฮาร์ดแวร์ ต่อ ZX-SWITCH จากช่อง LOW เข้ากับพอร์ต PA0

223 โปรแกรมทดสอบหุ่นยนต์ที่ 3 (switch1_test.c) อ่านค่าสถานะการทำงานของแผงสวิตช์ซึ่งต่อกับขาพอร์ต PA0 #include <sleep.h> #include <in_out.h> #include <lcd.h> void main() { while(1) lcd("Switch1 = %d ",in_a(0)); sleep(10); }

การทดลองที่ 4 (หุ่นยนต์หลบหลีกสิ่งกีดขวาง) 224 การทดลองที่ 4 (หุ่นยนต์หลบหลีกสิ่งกีดขวาง) หลักการทำงาน 1. ในกรณีที่ไม่เกิดการชน หุ่นยนต์จะเคลื่อนที่ไปข้างหน้าอย่างต่อเนื่อง 2. ในกรณีที่พบว่าเกิดการชน จะมีเสียงเตือนดัง 1 ครั้ง จากนั้นหุ่นยนต์จะเคลื่อนที่ถอยหลังแล้วเปลี่ยนทิศทางการเคลื่อนที่มาทางซ้าย

การเชื่อมต่อทางฮาร์ดแวร์ 225 การเชื่อมต่อทางฮาร์ดแวร์ 1. ต่อ ZX-SWITCH จากช่อง LOW เข้ากับพอร์ต PA0 2. ต่อ ZX-SPEAKER เข้ากับพอร์ต PB4

โปรแกรมทดสอบหุ่นยนต์ที่ 4 (robo_bumper1.c) หุ่นยนต์หลบหลีกสิ่งกีดขวาง 226 โปรแกรมทดสอบหุ่นยนต์ที่ 4 (robo_bumper1.c) หุ่นยนต์หลบหลีกสิ่งกีดขวาง #include <motor.h> #include <sleep.h> #include <in_out.h> #include <lcd.h> #include <sound.h> #define POW 70 void forward(unsigned int delay) { motor(1,POW); motor(2,POW); sleep(delay); } void backward(unsigned int delay) motor(1,-POW); motor(2,-POW); void turn_left(unsigned int delay) void main() { char mid; lcd("Press SW1"); sw1_press(); while(1) mid = in_a(0); if(mid==0) beep_b(4); backward(1000); turn_left(800); } else forward(10);

การทดลองที่ 5 (ตรวจจับการชนด้วยสวิตช์ 2 ตัว) 227 การทดลองที่ 5 (ตรวจจับการชนด้วยสวิตช์ 2 ตัว) การเชื่อมต่อทางฮาร์ดแวร์ 1. ต่อ ZX-SWITCH ตัวที่ 1 จากช่อง LOW เข้ากับพอร์ต PA0 2. ต่อ ZX-SWITCH ตัวที่ 2 จากช่อง LOW เข้ากับพอร์ต PA1

228 โปรแกรมทดสอบหุ่นยนต์ที่ 5 (switch2_test.c) ทดสอบการทำงานของแผงสวิตช์ 2 ตัวซึ่งต่อกับขาพอร์ต PA0 และ PA1 #include <sleep.h> #include <in_out.h> #include <lcd.h> void main() { while(1) lcd("Switch1 = %d #nSwitch2 = %d", in_a(0), in_a(1)); sleep(10); }

การทดลองที่ 6 (หุ่นยนต์หลบหลีกสิ่งกีดขวางจากตัวตรวจจับการชน 2 ตัว) 229 การทดลองที่ 6 (หุ่นยนต์หลบหลีกสิ่งกีดขวางจากตัวตรวจจับการชน 2 ตัว) หลักการทำงาน 1. ในกรณีที่หุ่นยนต์ไม่พบการชน หุ่นยนต์จะเคลื่อนที่ไปข้างหน้าอย่างต่อเนื่อง 2. กรณีที่หุ่นยนต์พบการชนของแผงวงจรสวิตช์ด้านซ้าย หุ่นยนต์จะส่งเสียงสัญญาณดัง 1 ครั้ง จากนั้นหุ่นยนต์เคลื่อนที่ถอยหลัง แล้วทำการเลี้ยวขวา 3. กรณีที่หุ่นยนต์พบการชนของแผงวงจรสวิตช์ด้านขวา หุ่นยนต์จะส่งเสียงสัญญาณดัง 1 ครั้ง จากนั้นหุ่นยนต์เคลื่อนที่ถอยหลัง แล้วทำการเลี้ยวซ้าย 4. กรณีที่หุ่นยนต์พบการชนของแผงวงจรสวิตช์ทั้งสองด้าน หุ่นยนต์จะส่งเสียงสัญญาณดัง 1 ครั้งเช่นกัน จากนั้นหุ่นยนต์เคลื่อนที่ถอยหลัง แล้วทำการเลี้ยวซ้าย

การเชื่อมต่อทางฮาร์ดแวร์ 230 การเชื่อมต่อทางฮาร์ดแวร์ 1. ต่อ ZX-SWITCH ตัวที่ 1 จากช่อง LOW เข้ากับพอร์ต PA0 2. ต่อ ZX-SWITCH ตัวที่ 2 จากช่อง LOW เข้ากับพอร์ต PA1 3. ต่อ ZX-SPEAKER เข้ากับพอร์ต PB4

231 โปรแกรมทดสอบหุ่นยนต์ที่ 6 (robo_bumper2.c) หุ่นยนต์หลบหลีกสิ่งกีดขวางโดยใช้ตัวตรวจจับการชน 2 ตัว #include <motor.h> #include <sleep.h> #include <in_out.h> #include <lcd.h> #include <sound.h> #define POW 70 void forward(unsigned int delay) { motor(1,POW); motor(2,POW); sleep(delay); } void backward(unsigned int delay) motor(1,-POW); motor(2,-POW); void turn_left(unsigned int delay) void turn_right(unsigned int delay) { motor(1,POW); motor(2,-POW); sleep(delay); } void main() char left,right; lcd("Press SW1"); sw1_press(); while(1) left = in_a(0); right = in_a(1); if(left==1 && right==1) forward(10); else if(left==0 && right==1) beep_b(4); backward(1000); turn_right(800); else if(left==1 && right==0) { beep_b(4); backward(1000); turn_left(800); } else if(left==0 && right==0) turn_left(1500);

การทดลองที่ 7 (หุ่นยนต์หลบหลีกสิ่งกีดขวางด้วยการชนแบบสุ่มทิศทาง) 232 การทดลองที่ 7 (หุ่นยนต์หลบหลีกสิ่งกีดขวางด้วยการชนแบบสุ่มทิศทาง) ฟังก์ชันสุ่มค่าตัวเลข rand() ฟังก์ชั่นสุ่มค่าตัวเลข ตั้งแต่ 0 ถึง 32767 หลักการทำงาน 1. ในกรณีที่หุ่นยนต์ไม่พบการชน (ไม่พบสิ่งกีดขวาง) หุ่นยนต์จะเคลื่อนที่ไปข้างหน้า 2. เมื่อหุ่นยนต์ตรวจพบว่าที่ด้านใดด้านหนึ่งหรือทั้งสองด้านพร้อมกัน จะส่งเสียงสัญญาณ 1 จังหวะ จากนั้นหุ่นยนต์เคลื่อนที่ถอยหลัง แล้วทำการสุ่มค่าเพื่อตัดสินใจเลือกทิศทางในการเลี้ยว

การเชื่อมต่อทางฮาร์ดแวร์ 233 การเชื่อมต่อทางฮาร์ดแวร์ 1. ต่อ ZX-SWITCH ตัวที่ 1 จากช่อง LOW เข้ากับพอร์ต PA0 2. ต่อ ZX-SWITCH ตัวที่ 2 จากช่อง LOW เข้ากับพอร์ต PA1 3. ต่อ ZX-SPEAKER เข้ากับพอร์ต PB4

234 โปรแกรมทดสอบหุ่นยนต์ที่ 7 (robo_bumper3.c) หุ่นยนต์หลบหลีกสิ่งกีดขวางโดยใช้วิธีการสุ่มทิศทาง #include <motor.h> #include <sleep.h> #include <in_out.h> #include <lcd.h> #include <sound.h> #define POW 70 void forward(unsigned int delay) { motor(1,POW); motor(2,POW); sleep(delay); } void backward(unsigned int delay) motor(1,-POW); motor(2,-POW); void turn_left(unsigned int delay) void turn_right(unsigned int delay) { motor(1,POW); motor(2,-POW); sleep(delay); } void main() char left,right; lcd("Press SW1"); sw1_press(); while(1) left = in_a(0); right = in_a(1); if(left==0 || right==0) beep_b(4); backward(1000); if(rand()%2==1) turn_left(800); else { turn_right(800); } forward(10);