บทท่ี 6IPST-BOT กับการหลบ

หลีกสิง่กีดขวางแบบสมัผัส

221

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

ต่อ ZX-SWITCH จากชอ่ง LOW เขา้กับพอรต์ PA0

222

โปรแกรมทดสอบหุ่นยนต์ท่ี 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); }}

223

การทดลองท่ี 4 (หุ่นยนต์หลบหลีกสิง่กีดขวาง)หลักการทำางาน

1. ในกรณีท่ีไมเ่กิดการชน หุ่นยนต์จะเคลื่อนท่ีไปขา้งหน้าอยา่งต่อเน่ือง

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

224

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

พอรต์ PA0 2 . ต่อ ZX-SPEAKER เขา้กับพอรต์ PB4

225

โปรแกรมทดสอบหุ่นยนต์ท่ี 4 (robo_bumper1.c) หุ่นยนต์หลบหลีกสิง่กีดขวาง#include <motor.h>

#include <sleep.h>#include <in_out.h>#include <lcd.h>#include <sound.h>#define POW 70void forward(unsigned int delay){ motor(1,POW); motor(2,POW); sleep(delay);} void backward(unsigned int delay){ motor(1,-POW); motor(2,-POW); sleep(delay);} void turn_left(unsigned int delay){ motor(1,-POW); motor(2,POW); sleep(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); } }}

226

การทดลองท่ี 5 (ตรวจจบัการชนด้วยสวติช ์ 2 ตัว) การเชื่อมต่อทางฮารด์แวร์

1. ต่อ ZX-SWITCH ตัวท่ี 1 จากชอ่ง LOW เขา้กับพอรต์ PA0

2. ต่อ ZX-SWITCH ตัวท่ี 2 จากชอ่ง LOW เขา้กับพอรต์ PA1

227

โปรแกรมทดสอบหุ่นยนต์ท่ี 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); }}

228

การทดลองท่ี 6 (หุ่นยนต์หลบหลีกสิง่กีดขวางจากตัวตรวจจบัการชน 2 ตัว)

หลักการทำางาน 1. ในกรณีท่ีหุ่นยนต์ไมพ่บการชน หุ่นยนต์จะ

เคล่ือนท่ีไปขา้งหน้าอยา่งต่อเนื่อง 2. กรณีท่ีหุ่นยนต์พบการชนของแผงวงจรสวติช์

ด้านซา้ย หุ่นยนต์จะสง่เสยีงสญัญาณดัง 1 ครัง้ จากนัน้หุ่นยนต์เคลื่อนท่ีถอยหลัง แล้วทำาการเล้ียวขวา

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

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

229

การเชื่อมต่อทางฮารด์แวร์ 1. ต่อ ZX-SWITCH ตัวท่ี 1 จากชอ่ง LOW

เขา้กับพอรต์ PA0 2. ต่อ ZX-SWITCH ตัวท่ี 2 จากชอ่ง LOW

เขา้กับพอรต์ PA1 3. ต่อ ZX-SPEAKER เขา้กับพอรต์ PB4

230

โปรแกรมทดสอบหุ่นยนต์ท่ี 6 (robo_bumper2.c) หุ่นยนต์หลบหลีกสิง่กีดขวางโดยใชตั้วตรวจจบัการชน 2 ตัว

#include <motor.h>#include <sleep.h>#include <in_out.h>#include <lcd.h>#include <sound.h>#define POW 70void forward(unsigned int delay){ motor(1,POW); motor(2,POW); sleep(delay);} void backward(unsigned int delay){ motor(1,-POW); motor(2,-POW); sleep(delay);} void turn_left(unsigned int delay){ motor(1,-POW); motor(2,POW); sleep(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) { beep_b(4); backward(1000); turn_left(1500); } }}

231

การทดลองท่ี 7 (หุ่นยนต์หลบหลีกสิง่กีดขวาง

ด้วยการชนแบบสุม่ทิศทาง) ฟงัก์ชนัสุม่ค่าตัวเลขrand() ฟงัก์ชัน่สุม่ค่าตัวเลข ตั้งแต่ 0 ถึง

32767

หลักการทำางาน 1. ในกรณีท่ีหุ่นยนต์ไมพ่บการชน (ไมพ่บ

สิง่กีดขวาง ) หุ่นยนต์จะเคลื่อนท่ีไปขา้งหน้า 2. เมื่อหุ่นยนต์ตรวจพบวา่ท่ีด้านใดด้าน

หน่ึงหรอืทั้งสองด้านพรอ้มกัน จะสง่เสยีงสญัญาณ 1 จงัหวะ จากนัน้หุ่นยนต์เคลื่อนท่ีถอยหลัง แล้วทำาการสุม่ค่าเพื่อตัดสนิใจเลือกทิศทางในการเล้ียว

232

การเชื่อมต่อทางฮารด์แวร์ 1. ต่อ ZX-SWITCH ตัวท่ี 1 จากชอ่ง LOW

เขา้กับพอรต์ PA0 2. ต่อ ZX-SWITCH ตัวท่ี 2 จากชอ่ง LOW

เขา้กับพอรต์ PA1 3. ต่อ ZX-SPEAKER เขา้กับพอรต์ PB4

233

โปรแกรมทดสอบหุ่นยนต์ท่ี 7 (robo_bumper3.c) หุ่นยนต์หลบหลีกสิง่กีดขวางโดยใชว้ธิกีารสุม่ทิศทาง

#include <motor.h>#include <sleep.h>#include <in_out.h>#include <lcd.h>#include <sound.h>#define POW 70void forward(unsigned int delay){ motor(1,POW); motor(2,POW); sleep(delay);} void backward(unsigned int delay){ motor(1,-POW); motor(2,-POW); sleep(delay);} void turn_left(unsigned int delay){ motor(1,-POW); motor(2,POW); sleep(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); } } else { forward(10); } }}

234