95mm 90mm 90mm 360g CPU PIC16F V [1] 2

Transcription

1

2 95mm 90mm 90mm 360g CPU PIC16F V [1] 2

3 A

4 7.1.3 B A 1 B 3 C 11 4

5 1 2 1

6 2 2

7

8 mm 16mm g 8mm 3mm 4

9 mm 16mm 3mm 5

10

11 3 ( 3.1) 10 ( 3.2)

12 PIC F A CPU V V CPU PIC16F877 LED 2 CPU 7.2V

13 V SW F C18 1 IN GND 2 OUT 3 47 F C17 5V LED R V 5V GP2S40 GP2S40 GP2S40 C1815 R19 10k R20 47k R30 R31 R32 R33 R34 R k k k 5V LED 5V RA2 RA1 RA0 VDD 12 2 MCLR R LED 5V C9 10 F 35 C F VDD RB5 42 C1815 R VDD 34 VSS 14 OEC1 16F RC5 43 RB6 R16 10k R17 47k R13 10k C1815 R14 47k VSS 41 RB V SW RD2 RD3 RD1 RD VSS 13 RB7 29 RC7 RC0 RC V 5V 5V 0.1 F C C16 Vs-B 10 F Vs-A VDD 4 OUT1-A 2 VIN2-A 6 OUT2-A 3 VIN1-A M C F VIN1-B 11 42D OUT1-B 13 VIN2-B C14 9 OUT2-B 4.7 F VSS VSS TA7279P M 8 7 C11 42D 4.7 F C F R29 R23 R26 20k 20k 20k R22 R25 C1815 C k 10k R28 C k R21 R24 R27 0.1µ T + - C C8 1000p 9 R9 R10 4.7k 10k 13 R11 4.7k 5V 14 4 VDD MCLR C1 + - R 1µ 1000p C2 C3 R2 2k R1 1µ R3 10k 10k 100k R p 10k OUT 3 + C4 R5 LM k R6-7 OUT µ S C5 D2 S D1 C6 R7 10k 0.01µ 5V R8 10k 3 RA4 RA RA RB3 CLKIN Vss 5 16F84A 2SC1815 VDD VSS 3.4 PIC16F877 PIC 16F84A 16F877-RC1 16F84A-RA2 9

14 3.1.2 TGIF 10

15 Kokushikan.Univ R T LED ' Œ ' Œ ˆ ' Œ ƒ ˇ' ˇ' IN 2B( ƒ2) IN 1B( ƒ1) IN 2A( 2) IN 1A( 1) LED LED

16 Kokushikan.Univ ' Œ ' Œ ˆ ' Œ ƒ ˇ' IN 2B( ƒ2) IN 1B( ƒ1) IN 2A( 2) IN 1A( 1) ƒ ˆ ` OUT1A( 1) OUT1B( ƒ1) OUT2A( 2) OUT2B( ƒ2) ˇ' ˇ'

17 Kokushikan.Univ ' Œ ' Œ ˆ ' Œ ƒ OUT1A( 1) OUT1B( ƒ1) OUT2A( 2) OUT2B( ƒ2) ˇ' ˇ'

18 4 90mm 95mm 90mm 360g 95mm 90mm 95mm

19 4.1 15

20 T/R kHz 100deg 16

21 GP2S05 LED LED CPU 17

22 LED,TLN108(F) TPS601A LED LED 18

23 S.L.T.JAPAN 42D 7mm 500gcm/5v 3.3Kgcm/5v rpm 19

24 5 C C Linax emacs PIC Ver.4 20

25 5.1 PIC16F877 input PIN PIC outoput PIN void [2] 1: /* * 2: * Filename: hukugou-ronbun.c --- 3: * 4: * Copyright (C) 2009 by Faculty of Engineering, Kokushikan University 5: * : Suzuki Youhei <youhei@deimos.comb.kokushikan.ac.jp> 6: * Time-stamp: <09/01/19 16:47:22 youhei> 7: * 8: * */ 9: 10: #include <16f877.h> 11: #fuses HS, NOWDT, PUT, NOPROTECT, BROWNOUT, NOLVP, NOCPD, NOWRT 12: 13: #use fast_io(a) 14: #use fast_io(b) 15: #use fast_io(c) 16: #use fast_io(d) 17: #use fast_io(e) 18: #use delay(clock = ) 19: #bit TOIF =0x0bb.2 20: 21: /* input */ 22: #define LIGHT_SENSE_FRONT_LEFT PIN_B7 // 23: #define LIGHT_SENSE_FRONT_RIGHT PIN_B4 // 24: #define LIGHT_SENSE_REAR PIN_C7 // 25: #define REFC_CNTR PIN_A1 // 26: #define REFC_LEFT PIN_A0 // 27: #define REFC_RIGT PIN_A2 // 28: #define RD_SONIC PIN_C0 // 29: /* output */ 30: #define LIGHT_OUT_FRONT PIN_B6 // LED 31: #define LIGHT_OUT_REAR PIN_C5 // LED 32: #define MT_RIGT_1 PIN_D1 // IN1 33: #define MT_RIGT_2 PIN_D0 // IN2 34: #define MT_LEFT_2 PIN_D3 // IN2 35: #define MT_LEFT_1 PIN_D2 // IN1 36: #define MONITOR_LED PIN_B5 // LED 37: #define RD_SONIC_STOP PIN_C1 // 38: /* reaction */ 39: #define NO_ACTION 0 // 0 40: #define GO_STRAIGHT 1 // 1 41: #define GO_STRAIGHT_FAST 2 // 2 42: #define GO_BACK 3 // 3 43: #define BRAKE 4 // 4 44: #define TURN_RIGHT 5 // 5 45: #define ATURN_RIGHT 6 // 6 46: #define BACK_RIGHT 7 // 7 47: #define TURN_LEFT 8 // 8 48: #define ATURN_LEFT 9 // 9 49: #define BACK_LEFT 10 // 10 50: #define MOTOR_STOP 11 // 11 51: 52: void setup()// 1 IN 0 OUT PIN 1 53: { 54: setup_adc_ports(no_analogs); 21

26 55: set_tris_a(0xff); // 0b A : set_tris_b(0x9f); // 0b B : set_tris_c(0xdd); // 0b C : set_tris_d(0xf0); // 0b D : set_tris_e(0xff); // 0b E : 61: } 62: 63: /* */ //void 64: 65: /* */ 66: void left_motor_back(){ output_high(mt_left_1); output_low(mt_left_2); } 67: /* */ 68: void left_motor_forward(){ output_low(mt_left_1); output_high(mt_left_2); } 69: /* */ 70: void left_motor_stop() { output_low(mt_rigt_1); output_low(mt_rigt_2); } 71: /* */ 72: void left_motor_brake() { output_high(mt_rigt_1); output_high(mt_rigt_2); } 73: /* */ 74: void right_motor_back() { output_low(mt_rigt_1); output_high(mt_rigt_2); } 75: /* */ 76: void right_motor_forward() { output_high(mt_rigt_1); output_low(mt_rigt_2); } 77: /* */ 78: void right_motor_stop() { output_low(mt_left_1); output_low(mt_left_2); } 79: /* */ 80: void right_motor_brake(){ output_high(mt_left_1); output_high(mt_left_2); } 81: 82: /* */ 83: void forward() { output_low(mt_left_1); output_high(mt_left_2); output_high(mt_rigt _1); output_low(mt_rigt_2); } 84: /* */ 85: void back() { output_high(mt_left_1); output_low(mt_left_2); output_low(mt_rigt_1); output_high(mt_rigt_2); } 86: /* */ 87: void right() { output_low(mt_rigt_1); output_high(mt_rigt_2); output_low(mt_left_1); output_high(mt_left_2); } 88: /* */ 89: void left() { output_high(mt_left_1); output_low(mt_left_2); output_high(mt_rigt_1); output_low(mt_rigt_2); } 90: /* */ 91: void stop(){ output_low( MT_RIGT_1); output_low(mt_rigt_2); output_low( MT_LEFT_2); o utput_low( MT_LEFT_1); } 92: /*LED */ 93: void MITUKO(){ output_high(monitor_led); } 94: /*LED */ 95: void SHOUKO(){ output_low(monitor_led); } 22

27 6 23

28 6.1 実験方法 単体制御動作実験方法を以下に示す 光を吸収する黒いマットを落差と見立て その上に路面となる光を反射する紙を置く 図 6.1 この上にロボットを配置し スタートさせ 落差を検出し回避行動を行うか検証する 図 6.1 前方 図 6.2 上部 また動作中に障害物となる物 図 6.3 を置き超音波センサの反応を検証する さらに障害物の種類を変化させ反応をみる 図 6.3 障害物 図 6.4 障害物２ 図 6.3 では硬くロボットに対して面であり 超音波の反射が強いと思われるティッ シュペーパーの箱 図 6.4 は円柱で柔らかいトイレットペーパーが少しの残ってい る芯を使用 24

29 NO NO NO NO YES YES YES YES

30 : /* * 2: * Filename: tantai dousa.c --- 3: * 4: * Copyright (C) 2008 by Faculty of Engineering, Kokushikan University 5: * : Suzuki Youhei <youhei@deimos.comb.kokushikan.ac.jp> 6: * Time-stamp: <08/11/13 15:29:53 youhei> 7: * 8: * */ 9: #include <16f877.h> 10: #fuses HS, NOWDT, PUT, NOPROTECT, BROWNOUT, NOLVP, NOCPD, NOWRT 11: 12: #use fast_io(a) 13: #use fast_io(b) 14: #use fast_io(c) 15: #use fast_io(d) 16: #use fast_io(e) 17: #use delay(clock = ) 18: #bit TOIF =0x0bb.2 19: 20: /* input */ 21: #define LIGHT_SENSE_FRONT_LEFT PIN_B7 // 22: #define LIGHT_SENSE_FRONT_RIGHT PIN_B4 // 23: #define LIGHT_SENSE_REAR PIN_C7 // 24: #define REFC_LEFT PIN_A1 // 25: #define REFC_CNTR PIN_A0 // 26: #define REFC_RIGT PIN_A2 // 27: #define RD_SONIC PIN_C0 // 28: /* output */ 29: #define LIGHT_OUT_FRONT PIN_B6 // LED 30: #define LIGHT_OUT_REAR PIN_C5 // LED 31: #define MT_RIGT_1 PIN_D1 // IN1 32: #define MT_RIGT_2 PIN_D0 // IN2 33: #define MT_LEFT_2 PIN_D3 // IN2 34: #define MT_LEFT_1 PIN_D2 // IN1 35: #define MONITOR_LED PIN_B5 // LED 36: /* reaction */ 37: #define NO_ACTION 0 // 0 38: #define GO_STRAIGHT 1 // 1 39: #define GO_STRAIGHT_FAST 2 // 2 40: #define GO_BACK 3 // 3 41: #define BRAKE 4 // 4 42: #define TURN_RIGHT 5 // 5 43: #define ATURN_RIGHT 6 // 6 44: #define BACK_RIGHT 7 // 7 45: #define TURN_LEFT 8 // 8 46: #define ATURN_LEFT 9 // 9 47: #define BACK_LEFT 10 // 10 48: #define MOTOR_STOP 11 // 11 49: 50: void setup()// 1 IN 0 OUT PIN 1 51: { 52: setup_adc_ports(no_analogs); 53: set_tris_a(0xff); // 0b A : set_tris_b(0x9f); // 0b B : set_tris_c(0xdf); // 0b C : set_tris_d(0xf0); // 0b D : set_tris_e(0xff); // 0b E : 59: } 60: 26

31 61: /* */ //void 62: 63: /* */ 64: void left_motor_back(){ output_high(mt_left_1); output_low(mt_left_2); } 65: /* */ 66: void left_motor_forward(){ output_low(mt_left_1); output_high(mt_left_2); } 67: /* */ 68: void left_motor_stop() { output_low(mt_rigt_1); output_low(mt_rigt_2); } 69: /* */ 70: void left_motor_brake() { output_high(mt_rigt_1); output_high(mt_rigt_2); } 71: /* */ 72: void right_motor_back() { output_low(mt_rigt_1); output_high(mt_rigt_2); } 73: /* */ 74: void right_motor_forward() { output_high(mt_rigt_1); output_low(mt_rigt_2); } 75: /* */ 76: void right_motor_stop() { output_low(mt_left_1); output_low(mt_left_2); } 77: /* */ 78: void right_motor_brake(){ output_high(mt_left_1); output_high(mt_left_2); } 79: 80: /* */ 81: void forward() { output_low(mt_left_1); output_high(mt_left_2); output_high(mt_rigt _1); output_low(mt_rigt_2); } 82: /* */ 83: void back() { output_high(mt_left_1); output_low(mt_left_2); output_low(mt_rigt_1); output_high(mt_rigt_2); } 84: /* */ 85: void right() { output_low(mt_rigt_1); output_high(mt_rigt_2); output_low(mt_left_1); output_high(mt_left_2); } 86: /* */ 87: void left() { output_high(mt_left_1); output_low(mt_left_2); output_high(mt_rigt_1); output_low(mt_rigt_2); } 88: /* */ 89: void stop(){ output_low( MT_RIGT_1); output_low(mt_rigt_2); output_low( MT_LEFT_2); o utput_low( MT_LEFT_1); } 90: // 91: void MITUKO(){ output_high(monitor_led); } 92: void SHOUKO(){ output_low(monitor_led); } 93: main() 94: { 95: setup(); 96: while(1) 97: { 98: forward(); 99: if(input(rd_sonic) == 1){ 100: MITUKO(); 101: stop(); 102: delay_ms(1000); 103: back(); 104: delay_ms(500); 105: right(); 106: delay_ms(1200); 107: } 108: else{ 109: SHOUKO(); 110: } 111: 112: if(input(refc_left) == 0){ 113: MITUKO(); 114: back(); 115: delay_ms(700); 116: right(); 117: delay_ms(1200); 118: } 119: else{ 27

32 120: SHOUKO(); 121: } 122: 123: if(iput(refc_cntr) == 0){ 124: MITUKO(); 125: right_motor_back(); 126: delay_ms(1200); 127: } 128: else{ 129: SHOUKO(); 130: } 131: 132: if(imput(refc_rigt) == 0){ 133: MITUKO(); 134: left_motor_back(); 135: delay_ms(1200); 136: } 137: else{ 138: SHOUKO(); 139: } 140: } 141: } 142: 143: 28

33 mm 4mm 29

34 7 30

35 7.1 実験方法 二体間通信実験方法を以下に示す 平面な床の上に 1 台を待機させ 赤外線を 後方に送信しながら前進するもう 1 台を置き 後方の待機しているロボットが前 方機の発する赤外線を受信したときのみ発信するようにプログラムを組み実験を 行う 図 7.1 後方機待機 図 7.2 図 追従 前方機設置

36 FRONT(B) BACK(A) 2 2 NO YES (A) 2. (B) A 3. B A 4. 32

37 7.1.2 A 1: /* * 2: * Filename: tusin.c --- 3: * 4: * Copyright (C) 2008 by Faculty of Engineering, Kokushikan University 5: * : Suzuki Youhei <youhei@deimos.comb.kokushikan.ac.jp> 6: * Time-stamp: <08/11/25 15:34:49 youhei> 7: * 8: * */ 9: #include <16f877.h> 10: #fuses HS, NOWDT, PUT, NOPROTECT, BROWNOUT, NOLVP, NOCPD, NOWRT 11: 12: #use fast_io(a) 13: #use fast_io(b) 14: #use fast_io(c) 15: #use fast_io(d) 16: #use fast_io(e) 17: #use delay(clock = ) 18: #bit TOIF =0x0bb.2 19: 20: /* input */ 21: #define LIGHT_SENSE_FRONT_LEFT PIN_B7 // 22: #define LIGHT_SENSE_FRONT_RIGHT PIN_B4 // 23: #define LIGHT_SENSE_REAR PIN_C7 // 24: #define REFC_LEFT PIN_A1 // 25: #define REFC_CNTR PIN_A0 // 26: #define REFC_RIGT PIN_A2 // 27: #define RD_SONIC PIN_C0 // 28: /* output */ 29: #define LIGHT_OUT_FRONT PIN_B6 // LED 30: #define LIGHT_OUT_REAR PIN_C5 // LED 31: #define MT_RIGT_1 PIN_D1 // IN1 32: #define MT_RIGT_2 PIN_D0 // IN2 33: #define MT_LEFT_2 PIN_D3 // IN2 34: #define MT_LEFT_1 PIN_D2 // IN1 35: #define MONITOR_LED PIN_B5 // LED 36: /* reaction */ 37: #define NO_ACTION 0 // 0 38: #define GO_STRAIGHT 1 // 1 39: #define GO_STRAIGHT_FAST 2 // 2 40: #define GO_BACK 3 // 3 41: #define BRAKE 4 // 4 42: #define TURN_RIGHT 5 // 5 43: #define ATURN_RIGHT 6 // 6 44: #define BACK_RIGHT 7 // 7 45: #define TURN_LEFT 8 // 8 46: #define ATURN_LEFT 9 // 9 47: #define BACK_LEFT 10 // 10 48: #define MOTOR_STOP 11 // 11 49: 50: void setup()// 1 IN 0 OUT PIN 1 51: { 52: setup_adc_ports(no_analogs); 53: set_tris_a(0xff); // 0b A : set_tris_b(0x9f); // 0b B : set_tris_c(0xdf); // 0b C : set_tris_d(0xf0); // 0b D : set_tris_e(0xff); // 0b E : 59: } 60: 33

38 61: /* */ //void 62: 63: /* */ 64: void left_motor_back(){ output_high(mt_left_1); output_low(mt_left_2); } 65: /* */ 66: void left_motor_forward(){ output_low(mt_left_1); output_high(mt_left_2); } 67: /* */ 68: void left_motor_stop() { output_low(mt_rigt_1); output_low(mt_rigt_2); } 69: /* */ 70: void left_motor_brake() { output_high(mt_rigt_1); output_high(mt_rigt_2); } 71: /* */ 72: void right_motor_back() { output_low(mt_rigt_1); output_high(mt_rigt_2); } 73: /* */ 74: void right_motor_forward() { output_high(mt_rigt_1); output_low(mt_rigt_2); } 75: /* */ 76: void right_motor_stop() { output_low(mt_left_1); output_low(mt_left_2); } 77: /* */ 78: void right_motor_brake(){ output_high(mt_left_1); output_high(mt_left_2); } 79: 80: /* */ 81: void forward() { output_low(mt_left_1); output_high(mt_left_2); output_high(mt_rigt _1); output_low(mt_rigt_2); } 82: /* */ 83: void back() { output_high(mt_left_1); output_low(mt_left_2); output_low(mt_rigt_1); output_high(mt_rigt_2); } 84: /* */ 85: void right() { output_low(mt_rigt_1); output_high(mt_rigt_2); output_low(mt_left_1); output_high(mt_left_2); } 86: /* */ 87: void left() { output_high(mt_left_1); output_low(mt_left_2); output_high(mt_rigt_1); output_low(mt_rigt_2); } 88: /* */ 89: void stop(){ output_low( MT_RIGT_1); output_low(mt_rigt_2); output_low( MT_LEFT_2); o utput_low( MT_LEFT_1); } 90: // 91: void MITUKO(){ output_high(monitor_led); } 92: void SHOUKO(){ output_low(monitor_led); } 93: main() 94: { 95: setup(); 96: while(1) 97: { 98: if(input(light_sense_front_left)!= 0 && input(light_sense_front_right)! = 0){ 99: forward(); 100: MITUKO(); 101: }else{ 102: stop(); 103: SHOUKO(); 104: } 105: } 106: } 34

39 7.1.3 B B : main() 94: { 95: setup(); 96: while(1) 97: { 98: forward(); 99: 100: MITUKO(); 101: output_high(light_out_rear); 102: delay_ms(1000); 103: stop(); 104: output_low(light_out_rear); 105: delay_ms(1000); 106: SHOUKO(); 107: } 108: } 35

40 7.2 結果 二体間通信実験を行った結果以下のことがわかった 赤外線を送信しながら前 進する前方機は単体動作実験の結果からもわかる通り直進安定性が悪く 長時間 直進すると左右のどちらかに徐々に曲がっていく 個体により左右は変わる こ のことにより前方機が曲がり始めると ロボット同士に角度がつき後方機は前方 機の赤外線送信範囲外に入り情報の受信することが出来なくなってしまう 図 7.5 これを改善するために次項の二体間通信実験 2 を行った 図 断裂

41 8 2 A 37

42 8.1 実験方法 二体間通信実験 2 の方法を以下に示す 前項の二体間通信実験と同様に平面な 床の上に 1 台を待機させ 赤外線を後方に送信しながら前進するもう 1 台を置き 後方の待機しているロボットが前方機の発する赤外線を二つのフォトトランジス タが受信したときのみ前進し 前方機との隊列が乱れたときは後方機が隊列を組 み直すようにプログラムを組み実験を行う 図 8.1 二体間通信実験 2 38

43 FRONT(B) BACK(A) 2 NO YES NO YES NO YES A

44 : void tusin_back(){ 104: if(input(light_sense_front_left) == 1 && input(light_sense_front_right) == 1){ 105: ; 106: 107: }else if(input(light_sense_front_left)!= 1 && input(light_sense_front_right) == 1){ 108: while(input(light_sense_front_left)!= 1){ 109: MITUKO(); 110: stop(); 111: right_motor_forward(); 112: } 113: }else if(input(light_sense_front_left) == 1 && input(light_sense_front_right)!= 1){ 114: while(input(light_sense_front_right)!= 1){ 115: MITUKO(); 116: stop(); 117: left_motor_forward(); 118: } 119: }else{ 120: SHOUKO(); 121: } 122: }

45 8.2 結果 以下に二体間通信実験 2 の結果を示す プログラムを改良し検証した結果 後 方機が軌道を修正して前方機に追従して行くことが可能になった 追従距離は格 段に延び路面が続く限り追従していける事もわかった この実験結果を受け 1 台 のロボットに前方機 後方機として機能するようにプログラムを書き込んだとこ ろ ロボットの前後を入れ換えても正常に動作した 図 8.3 二体間通信実験 2 結果 41

46 9 2 42

47 9.1 実験方法 複合動作実験の方法を以下に示す 光を吸収する黒いマットを落差と見立て そ の上に路面となる光を反射する紙を置く この上にロボットを 2 台ランダムに配 置しスタートさせ落差を検出し 回避行動を行いながら二台が仲間を発見し追従 するか検証する 図 9.1 複合動作実験スタート 図 9.2 経過 スタートさせた後 追従することが出来るかどうか検証する 図 9.3 経過 2 43

48 YES YES YES NO NO NO NO YES NO YES YES NO YES NO NO YES NO YES NO YES

49 9.3 1: /* * 2: * Filename: hukugou.c --- 3: * 4: * Copyright (C) 2008 by Faculty of Engineering, Kokushikan University 5: * : Suzuki Youhei <youhei@deimos.comb.kokushikan.ac.jp> 6: * Time-stamp: <08/12/15 20:08:27 youhei> 7: * 8: * */ 9: #include <16f877.h> 10: #fuses HS, NOWDT, PUT, NOPROTECT, BROWNOUT, NOLVP, NOCPD, NOWRT 11: 12: #use fast_io(a) 13: #use fast_io(b) 14: #use fast_io(c) 15: #use fast_io(d) 16: #use fast_io(e) 17: #use delay(clock = ) 18: #bit TOIF =0x0bb.2 19: 20: /* input */ 21: #define LIGHT_SENSE_FRONT_LEFT PIN_B7 // 22: #define LIGHT_SENSE_FRONT_RIGHT PIN_B4 // 23: #define LIGHT_SENSE_REAR PIN_C7 // 24: #define REFC_CNTR PIN_A1 // 25: #define REFC_LEFT PIN_A0 // 26: #define REFC_RIGT PIN_A2 // 27: #define RD_SONIC PIN_C0 // 28: /* output */ 29: #define LIGHT_OUT_FRONT PIN_B6 // LED 30: #define LIGHT_OUT_REAR PIN_C5 // LED 31: #define MT_RIGT_1 PIN_D1 // IN1 32: #define MT_RIGT_2 PIN_D0 // IN2 33: #define MT_LEFT_2 PIN_D3 // IN2 34: #define MT_LEFT_1 PIN_D2 // IN1 35: #define MONITOR_LED PIN_B5 // LED 36: #define RD_SONIC_STOP PIN_C1 // 37: /* reaction */ 38: #define NO_ACTION 0 // 0 39: #define GO_STRAIGHT 1 // 1 40: #define GO_STRAIGHT_FAST 2 // 2 41: #define GO_BACK 3 // 3 42: #define BRAKE 4 // 4 43: #define TURN_RIGHT 5 // 5 44: #define ATURN_RIGHT 6 // 6 45: #define BACK_RIGHT 7 // 7 46: #define TURN_LEFT 8 // 8 47: #define ATURN_LEFT 9 // 9 48: #define BACK_LEFT 10 // 10 49: #define MOTOR_STOP 11 // 11 50: 51: void setup()// 1 IN 0 OUT PIN 1 52: { 53: setup_adc_ports(no_analogs); 54: set_tris_a(0xff); // 0b A : set_tris_b(0x9f); // 0b B : set_tris_c(0xdd); // 0b C : set_tris_d(0xf0); // 0b D : set_tris_e(0xff); // 0b E : 60: } 45

50 61: 62: /* */ //void 63: 64: /* */ 65: void left_motor_back(){ output_high(mt_left_1); output_low(mt_left_2); } 66: /* */ 67: void left_motor_forward(){ output_low(mt_left_1); output_high(mt_left_2); } 68: /* */ 69: void left_motor_stop() { output_low(mt_rigt_1); output_low(mt_rigt_2); } 70: /* */ 71: void left_motor_brake() { output_high(mt_rigt_1); output_high(mt_rigt_2); } 72: /* */ 73: void right_motor_back() { output_low(mt_rigt_1); output_high(mt_rigt_2); } 74: /* */ 75: void right_motor_forward() { output_high(mt_rigt_1); output_low(mt_rigt_2); } 76: /* */ 77: void right_motor_stop() { output_low(mt_left_1); output_low(mt_left_2); } 78: /* */ 79: void right_motor_brake(){ output_high(mt_left_1); output_high(mt_left_2); } 80: 81: /* */ 82: void forward() { output_low(mt_left_1); output_high(mt_left_2); output_high(mt_rigt _1); output_low(mt_rigt_2); } 83: /* */ 84: void back() { output_high(mt_left_1); output_low(mt_left_2); output_low(mt_rigt_1); output_high(mt_rigt_2); } 85: /* */ 86: void right() { output_low(mt_rigt_1); output_high(mt_rigt_2); output_low(mt_left_1); output_high(mt_left_2); } 87: /* */ 88: void left() { output_high(mt_left_1); output_low(mt_left_2); output_high(mt_rigt_1); output_low(mt_rigt_2); } 89: /* */ 90: void stop(){ output_low( MT_RIGT_1); output_low(mt_rigt_2); output_low( MT_LEFT_2); o utput_low( MT_LEFT_1); } 91: // 92: void MITUKO(){ output_high(monitor_led); } 93: void SHOUKO(){ output_low(monitor_led); } 94: 95: void tusin_back(){ 96: if(input(light_sense_front_left) == 1 && input(light_sense_front_right) == 1){ 97: while(input(light_sense_front_left) == 1 && input(light_sense_front_right) == 1){ 98: output_high(light_out_front); 99: output_high(rd_sonic_stop); 100: } 101: }else if(input(light_sense_front_left)!= 1 && input(light_sense_front_right) == 1){ 102: while(input(light_sense_front_left)!= 1){ 103: MITUKO(); 104: output_high(light_out_front); 105: output_high(rd_sonic_stop); 106: stop(); 107: right_motor_forward(); 108: } 109: }else if(input(light_sense_front_left) == 1 && input(light_sense_front_right)!= 1){ 110: while(input(light_sense_front_right)!= 1){ 111: MITUKO(); 112: output_high(light_out_front); 113: output_high(rd_sonic_stop); 114: stop(); 115: left_motor_forward(); 116: } 46

51 117: }else{ 118: SHOUKO(); 119: } 120: } 121: void tusin_front(){ 122: output_high(light_out_rear); 123: if(input(light_sense_rear) == 1){ 124: 125: while(input(light_sense_rear) == 1){ 126: MITUKO(); 127: output_high(light_out_rear); 128: forward(); 129: fotorifu(); 130: tyouonpa(); 131: } 132: 133: }else{ 134: SHOUKO(); 135: } 136: } 137: void fotorifu(){ 138: 139: if(input(refc_cntr) == 0){ 140: MITUKO(); 141: back(); 142: delay_ms(700); 143: right(); 144: delay_ms(1200); 145: } 146: else{ 147: SHOUKO(); 148: } 149: 150: if(input(refc_left) == 0){ 151: MITUKO(); 152: right_motor_back(); 153: delay_ms(1200); 154: } 155: else{ 156: SHOUKO(); 157: } 158: 159: if(input(refc_rigt) == 0){ 160: MITUKO(); 161: left_motor_back(); 162: delay_ms(1200); 163: } 164: else{ 165: SHOUKO(); 166: } 167: } 168: 169: 170: 171: void tyouonpa(){ 172: if(input(rd_sonic) == 1){ 173: MITUKO(); 174: if(input(light_sense_front_left) == 1 input(light_sense_front_right) == 1) { 175: 176: tusin_back(); 177: } 178: 179: stop(); 47

52 180: delay_ms(1000); 181: back(); 182: delay_ms(500); 183: right(); 184: delay_ms(1200); 185: 186: }else{ 187: SHOUKO(); 188: } 189: } 190: 191: main() 192: { 193: setup(); 194: while(1) 195: { 196: forward(); 197: tusin_back(); 198: tusin_front(); 199: fotorifu(); 200: tyouonpa(); 201: 202: } 203: 204: } 48

53 9.4 結果 以下に実験結果を示す 複合動作実験をおこなった結果以下の問題点がわかった 1. 障害物と仲間を区別できないためロボットを認識すると回避行動を行ってし まう 2. 同じ超音波センサを使用しているために 前方 後方に他機がいる際に障害 物があると認識してしてしまう 3. 超音波センサの指向性の問題から指向範囲外から近付いてきたロボットを認 識できないため追突してしまう (図 9.5) 4. 通信用のセンサが前後に配置されているために 前後に並んだ時であり さ らに超音波を感知しなかったときでしか追従できず その確立は極めて低い 実験では追従することは出来なかった 5. 障害物 落差回避行動中は他のセンサを停止させているためロボットの接近 や接触を認識し回避することが出来ないことがわかった 図 追突

54

55

56 [1] [2] C

57 A A.1 16F877 PIC 16F877 PLCC 8k 368 Flash I/O 33 20MHz A/D USART - - CCP - SSP - WDT A.2 TPS601A 0.1min[mA] 0.1[mW/cm 2 ] 0.2max[µA] 30[V] 800[nm] [deg] 2/2(tr/tf)[]µs] 1

58 A.3 LED TLN108(F) [ma] 940 [nm] 1.3[V] 100[mA] A.4 STLJAPAN HS-GM2-DSD [mm] φ3 10[mm] DC [V] DC [V] [mA] 3300[gcm]/DC 5[V] 500[gcm]/DC 5[V] [rpm] A.5 T/R dB 40[kHz] 107 [db] -70 [db] 100 [deg] [pf] A.6 4[mm] LED 1.2[V] 140[µA] RL = 47[kΩ] Vcc = 5[V] 3[µA] RL = 47[kΩ] Vcc = 5[V] 2

59 B 1: /* * 2: * Filename: moving.c --- 3: * 4: * Copyright (C) 2009 by Faculty of Engineering, Kokushikan University 5: * : Ken Kishimto <ken@pluto.home.chofu.tokyo.jp> 6: * Time-stamp: <09/02/03 17:22:28 youhei> 7: * 8: * */ 9: // #include <16f877.h> 10: # include "linuxpic.h" 11: 12: #define LIGHT_SENSE_FRONT_LEFT PIN_B7 // OOM 13: #define LIGHT_SENSE_FRONT_RIGHT PIN_B4 // OEOM 14: #define LIGHT_SENSE_REAR PIN_C7 // OM 15: #define REFC_CNTR PIN_A1 // tn^ 16: #define REFC_LEFT PIN_A0 // tn^ 17: #define REFC_RIGT PIN_A2 // tn^ E 18: #define RD_SONIC PIN_C0 // g 19: /* output */ 20: #define LIGHT_OUT_FRONT PIN_B6 // OO LED 21: #define LIGHT_OUT_REAR PIN_C5 // O LED 22: #define MT_RIGT_1 PIN_D1 // E[^[ IN1 23: #define MT_RIGT_2 PIN_D0 // E[^[ IN2 24: #define MT_LEFT_2 PIN_D3 // [^[ IN2 25: #define MT_LEFT_1 PIN_D2 // [^[ IN1 26: #define MONITOR_LED PIN_B5 // j^[led 27: #define RD_SONIC_STOP PIN_C1 // g~ 28: 29: /* reaction */ 30: #define NO_ACTION 0 // 0 31: #define GO_STRAIGHT 1 // 1 i 32: #define GO_STRAIGHT_FAST 2 // 2 i 33: #define GO_BACK 3 // 3 34: #define BRAKE 4 // 4 u[l 35: #define TURN_RIGHT 5 // 5 E^[ 36: #define ATURN_RIGHT 6 // 6 E^[ 37: #define BACK_RIGHT 7 // 7 EobN^[ 38: #define TURN_LEFT 8 // 8 ^[ 39: #define ATURN_LEFT 9 // 9 ^[ 40: #define BACK_LEFT 10 // 10 obn^[ 41: #define MOTOR_STOP 15 // 15 ~ 42: #define END 99 // 15 ~ 43: 44: #define ON 1 45: #define OFF 0 46: 47: #define NACTION 8 // sl 48: 49: int status, reaction; 50: 51: // sli 52: int32 counter, // sp 3

60 53: countq[naction]; // sp 54: int action, // s 55: actque[naction+1];// ss 56: 57: int1 lgt_fl, lgt_fr, lgt_r, sonic; // ZT 58: int1 ref_l, ref_r, ref_c; // ZT 59: int1 lgtout_f, lgtout_r, out_sonic; 60: 61: void setup(); 62: void left_motor_back(); 63: void left_motor_forward(); 64: void left_motor_stop(); 65: void left_motor_brake(); 66: void right_motor_back(); 67: void right_motor_forward(); 68: void right_motor_stop(); 69: void right_motor_brake(); 70: void forward(); 71: void back(); 72: void aright(); 73: void aleft(); 74: void right(); 75: void left(); 76: void stop(); 77: void stop_with_led(); 78: void LED_on(); 79: void LED_off(); 80: int8 sonic_sense(); 81: int8 sensing(); 82: void rear_lamp(int1 s); 83: void front_lamp(int1 s); 84: int8 sense_rear_lgt(); 85: void flush_front_lamp(int mod); 86: void flush_rear_lamp(int mod); 87: 88: /* 89: #fuses HS, NOWDT, PUT, NOPROTECT, BROWNOUT, NOLVP, NOCPD, NOWRT 90: 91: #use fast_io(a) 92: #use fast_io(b) 93: #use fast_io(c) 94: #use fast_io(d) 95: #use fast_io(e) 96: #use delay(clock = ) 97: #bit TOIF =0x0bb.2 98: */ 99: /* * 100: * Filename: moving.c : * 102: * Copyright (C) 2009 by Faculty of Engineering, Kokushikan University 103: * : Ken Kishimto <ken@pluto.home.chofu.tokyo.jp> 104: * Time-stamp: <09/01/29 16:33:46 youhei> 105: * 106: * */ 107: void setup() //B1IN0OUTBgPIN1B 108: { 109: setup_adc_ports(no_analogs); 110: set_tris_a(0xff); // 0b EA0B16i@ 111: set_tris_b(0x9f); // 0b EB0B16i@ 112: set_tris_c(0xdf); // 0b EC0B16i@ 113: set_tris_d(0xf0); // 0b ED0B16i@ 114: set_tris_e(0xff); // 0b EE0B16i@ 115: } 116: 4

61 117: /* { n[ho */ //void gb 118: /* obn */ 119: void left_motor_back() 120: { output_high(mt_left_1); output_low(mt_left_2); } 121: /* Oi */ 122: void left_motor_forward() 123: { output_low(mt_left_1); output_high(mt_left_2); } 124: /* Xgbv */ 125: void left_motor_stop() 126: { 127: output_low(mt_rigt_1); output_low(mt_rigt_2); 128: } 129: /* u[l */ 130: void left_motor_brake() 131: { output_high(mt_rigt_1); output_high(mt_rigt_2); } 132: /* EobN */ 133: void right_motor_back() 134: { output_low(mt_rigt_1); output_high(mt_rigt_2); } 135: /* EOi */ 136: void right_motor_forward() 137: { output_high(mt_rigt_1); output_low(mt_rigt_2); } 138: /* EXgbv */ 139: void right_motor_stop() 140: { output_low(mt_left_1); output_low(mt_left_2); } 141: /* Eu[L */ 142: void right_motor_brake() 143: { output_high(mt_left_1); output_high(mt_left_2); } 144: 145: /* Oi */ 146: void forward() 147: { 148: left_motor_forward(); 149: right_motor_forward(); 150: } 151: /* */ 152: void back() 153: { 154: left_motor_back(); 155: right_motor_back(); 156: } 157: /* v */ 158: void aright() 159: { 160: left_motor_forward(); 161: right_motor_back(); 162: } 163: /* v */ 164: void aleft() 165: { 166: right_motor_forward(); 167: left_motor_back(); 168: } 169: /* x_v */ 170: void right() 171: { 172: left_motor_forward(); 173: right_motor_stop(); 174: } 175: /* Ex_v */ 176: void left() 177: { 178: right_motor_forward(); 179: left_motor_stop(); 180: } 5

62 181: /* ~ */ 182: void stop() 183: { 184: right_motor_stop(); 185: left_motor_stop(); 186: } 187: // 188: void LED_on() 189: { 190: output_high(monitor_led); 191: } 192: void LED_off() 193: { 194: output_low(monitor_led); 195: } 196: 197: int8 sonic_sense() 198: { 199: output_high(rd_sonic_stop); 200: delay_us(500); 201: sonic = input(rd_sonic); 202: output_low(rd_sonic_stop); 203: return sonic; 204: } 205: /* x~ */ 206: void stop_with_led() 207: { 208: stop(); 209: LED_off(); 210: delay_ms(5000); 211: LED_on(); 212: delay_ms(5000); 213: } 214: 215: /* ZTXL */ 216: int8 sensing() 217: { 218: int8 a = 0; 219: 220: lgt_fl = input(light_sense_front_left); 221: a += lgt_fl; 222: lgt_fr = input(light_sense_front_right); 223: a += lgt_fr; 224: lgt_r = input(light_sense_rear); 225: a += lgt_r; 226: sonic = sonic_sense(); 227: a += sonic; 228: ref_c = 1 - input(refc_cntr); 229: a += ref_c; 230: ref_l = 1 - input(refc_left); 231: a += ref_l; 232: ref_r = 1 - input(refc_rigt); 233: a += ref_r; 234: return a; 235: } 236: 237: void rear_lamp(int1 s) 238: { 239: if (s) 240: output_high(light_out_rear); 241: else 242: output_low(light_out_rear); 243: } 244: 6

63 245: void front_lamp(int1 s) 246: { 247: if (s) 248: output_high(light_out_front); 249: else 250: output_low(light_out_front); 251: } 252: int8 sense_rear_lgt() 253: { 254: lgt_r = input(light_sense_rear); 255: return lgt_r; 256: } 257: 258: void flush_front_lamp(int mod) 259: { 260: int i; 261: 262: switch (mod) { 263: case 0: // " " 264: for (i = 0; i < 4; i++) { 265: front_lamp(1); 266: delay_ms(30); 267: } 268: break; 269: case 1: // "...." 270: for (i = 0; i < 4; i++) { 271: front_lamp(1); 272: delay_ms(5); 273: front_lamp(0); 274: delay_ms(25); 275: } 276: break; 277: case 2: // " " 278: for (i = 0; i < 4; i++) { 279: front_lamp(1); 280: delay_ms(25); 281: front_lamp(0); 282: delay_ms(5); 283: } 284: break; 285: default: 286: break; 287: } 288: front_lamp(0); 289: } 290: 291: void flush_rear_lamp(int mod) 292: { 293: int i; 294: 295: switch (mod) { 296: case 0: // " " 297: for (i = 0; i < 4; i++) { 298: rear_lamp(1); 299: delay_ms(30); 300: } 301: break; 302: case 1: // "...." 303: for (i = 0; i < 4; i++) { 304: rear_lamp(1); 305: delay_ms(5); 306: rear_lamp(0); 307: delay_ms(25); 308: } 7

64 309: break; 310: case 2: // " " 311: for (i = 0; i < 4; i++) { 312: rear_lamp(1); 313: delay_ms(25); 314: rear_lamp(0); 315: delay_ms(5); 316: } 317: break; 318: default: 319: break; 320: } 321: rear_lamp(0); 322: } 323: /* */ 324: void 325: do_action(int act) 326: { 327: switch (act) { 328: case GO_STRAIGHT : forward(); break; 329: case GO_STRAIGHT_FAST: forward(); break; 330: case GO_BACK : back(); break; 331: case BRAKE : left_motor_brake(); 332: right_motor_brake(); break; 333: case TURN_RIGHT : right(); break; 334: case ATURN_RIGHT : aright(); break; 335: case BACK_RIGHT : right_motor_back(); break; 336: case TURN_LEFT : left(); break; 337: case ATURN_LEFT : aleft(); break; 338: case BACK_LEFT : left_motor_back(); break; 339: case MOTOR_STOP : stop(); break; 340: case NO_ACTION : 341: case END : stop_with_led(); 342: default : break; 343: } 344: } 345: /* 346: * VANVL 347: * NACTION Yp 348: */ 349: void rememb_action(int act, int32 conu) 350: { 351: int i; 352: 353: for (i = NACTION - 1; i > 0; i--) { 354: actque[i] = actque[i - 1]; 355: countq[i] = countq[i - 1]; 356: } 357: actque[0] = act; 358: countq[0] = conu; 359: actque[naction] = END; 360: } 361: /* 362: * OMF 363: */ 364: void commun_front() 365: { 366: if (lgt_fl == 1 lgt_fr == 1) { 367: LED_on(); 368: flush_front_lamp(1); 369: counter = 500; 370: if (lgt_fl == 1 && lgt_fr == 1) { 371: action = GO_STRAIGHT; 372: } else if (lgt_fr == 1) { 8

65 373: action = TURN_LEFT; 374: } else { 375: action = TURN_RIGHT; 376: } 377: } else { 378: LED_off(); 379: } 380: } 381: /* 382: * MFM 383: */ 384: void commun_back() 385: { 386: flush_rear_lamp(1); 387: if (sense_rear_lgt()) { 388: action = GO_STRAIGHT; 389: counter = 5000; 390: } 391: } 392: /* 393: * 394: */ 395: int main() 396: { 397: int i, em; 398: 399: setup(); 400: for (i = 0; i < NACTION; i++) { 401: actque[i] = END; 402: countq[i] = 0; 403: } 404: action = GO_STRAIGHT; 405: counter = 32767; 406: while (1) { 407: do_action(action); 408: if (sensing()) { 409: em = 0; // } 410: LED_on(); 411: /* Q dv*/ 412: if (sonic) { 413: action = MOTOR_STOP; 414: counter = 1000; 415: actque[0] = GO_BACK; 416: countq[0] = 500; 417: actque[1] = TURN_LEFT; 418: countq[1] = 1200; 419: actque[2] = END; 420: em = 1; // } 421: } 422: if (ref_c) { 423: action = GO_BACK; 424: counter = 2400; // 1200ms 425: actque[0] = TURN_RIGHT; 426: countq[0] = 1200; 427: actque[1] = GO_STRAIGHT; 428: countq[1] = 32767; // 429: em = 1; // } 430: } 431: if (ref_r && ref_l) { 432: action = GO_BACK; 433: counter = 2400; // 1200ms 434: actque[0] = ATURN_RIGHT; 435: countq[0] = 1200; 436: actque[1] = GO_STRAIGHT; 9

66 437: countq[1] = 32767; // 438: em = 1; // } 439: } else if (ref_r) { 440: action = BACK_LEFT; 441: counter = 1200; // 1200ms 442: actque[0] = GO_BACK; 443: countq[0] = 1200; // 1200ms 444: actque[1] = END; 445: em = 1; // } 446: } else if (ref_l) { 447: action = BACK_RIGHT; 448: counter = 1200; // 1200ms 449: actque[0] = GO_BACK; 450: countq[0] = 1200; // 1200ms 451: actque[1] = END; 452: em = 1; // } 453: } 454: if (em == 0) { // } 455: /* M : mmbtbi */ 456: commun_back(); 457: commun_front(); 458: } 459: } else { 460: LED_off(); 461: if (counter > 0) { // sip 462: counter--; 463: } else { 464: counter = countq[0]; // so 465: action = actque[0]; 466: if (action!= END) { // svob 467: for (i = 0; i < NACTION - 1; i++) { 468: actque[i] = actque[i+1]; 469: countq[i] = countq[i+1]; 470: } 471: actque[naction] = END; 472: } 473: } 474: } 475: if (action == END) { // SsIFftHgs 476: // OiH 477: action = GO_STRAIGHT; 478: counter = 100; 479: // sdv 480: actque[0] = ATURN_LEFT; 481: countq[0] = 200; 482: actque[1] = END; 483: } 484: } 485: return 0; 486: } 487: /* */ 10

67 C Linax OS windows Linax windows OS PIC IC C 11

68 C

69