' {\$STAMP BS2} ' {\$PBASIC 2.5} ' straight bkwd: 675/850 ' straight fwd: 815/650 ' 90 deg: left-18/660, right-18/840 '20 pulses on power adapter are app. 10-11cm on straight fwd '*** vars *** ir_left VAR Bit: ir_front VAR Bit 'brick detection speed VAR Byte: i VAR Word ir_1 VAR Bit: ir_2 VAR Bit: ir_3 VAR Bit: ir_4 VAR Bit: ir_5 VAR Bit 'ir_1 is the leftmost one brick_f VAR Bit: brick_l VAR Bit p13 VAR Word: p12 VAR Word: pcount VAR Word deb VAR Word: debf VAR Nib: debl VAR Nib gap_cnt VAR Byte '*** start *** FREQOUT 4,100, 2000: PAUSE 50: FREQOUT 4,100, 2000 'start/reset brick_f = 0: brick_l = 0 deb = 0: gap_cnt = 0 speed=100 '*** main *** start: 'PAUSE 500 DO 'waiting for hand signal to start GOSUB measure2 LOOP UNTIL brick_l FREQOUT 4,300, 1500: PAUSE 1000 brick_f=0: brick_l=0 DO GOSUB measure GOSUB measure2 'DEBUG "brick? ", BIN brick_f 'IF brick_l THEN: brick_l=0: GOTO start: ENDIF IF brick_f=0 THEN GOSUB move ELSE GOSUB brick ENDIF LOOP END '*** sub *** measure: ir_1 = IN5: ir_2 = IN6: ir_3 = IN7: ir_4 = IN8: ir_5 = IN9 RETURN measure2: FREQOUT 0, 1, 36000 'front ir led, 37000 instead of 38500 to detect objects closer ir_front = IN1 FREQOUT 14, 1, 36000 'left ir led, freq. set to les than 38500 to detect the brick correctly ir_left = IN15 IF ir_front=0 THEN 'filtering debf=debf+1: IF debf>5 THEN: brick_f=1: ENDIF ELSE debf=0 ENDIF IF ir_left=0 THEN 'filtering debl=debl+1: IF debl>5 THEN: brick_l=1: ENDIF ELSE debl=0 ENDIF RETURN move: IF ir_1=0 AND ir_2=0 AND ir_3=0 AND ir_4=0 AND ir_5=0 THEN 'there is no line ahead 'FREQOUT 4,5,1000 gap_cnt = gap_cnt +1 IF gap_cnt > 25 THEN PAUSE 200: p13=650: p12=850:pcount=23:GOSUB mv_sub 'stop and bkwd GOSUB locate_line: gap_cnt=0 ENDIF ENDIF IF ir_1 AND ir_2 AND ir_3 AND ir_4 AND ir_5 THEN 'to ignore the "start line" GOSUB mv_fwd ELSEIF ir_1 THEN: GOSUB mv_right_more ELSEIF ir_2 THEN: GOSUB mv_right ELSEIF ir_5 THEN: GOSUB mv_left_more ELSEIF ir_4 THEN: GOSUB mv_left ELSE GOSUB mv_fwd ENDIF 'IF deb=100 THEN: FREQOUT 4,50,4000: deb=0: ENDIF: deb = deb + 1 'used for measuring loop time 'PAUSE 16 ' *** TO DO ***, check whole loop time and alter this pause to optimize servo speed 'PS: without motors it is app. 2,74s, with motors app. 3,16s 'BATT (used): 3,71s w/o motors, 3,69s w motors RETURN move2: IF ir_1 OR ir_3 OR ir_5 THEN GOSUB mv_sub_b_short GOSUB mv_trn_r 'this used mv_trn_r with 21 pulses brick_f = 0 RETURN ELSE GOSUB mv_fwd ENDIF PAUSE 20 RETURN brick: 'not using move2 subroutine GOSUB mv_sub_r: GOSUB mv_sub_f_short GOSUB mv_sub_arc1 'first part, hopefully no line yet 'PAUSE 2000 GOSUB mv_sub_arc2 'second part, if there is line, it is curved right IF ir_1 OR ir_3 OR ir_5 THEN brick_f = 0: GOSUB mv_sub_b_short: GOSUB locate_line GOSUB mv_trn_r: RETURN ENDIF 'PAUSE 2000 GOSUB mv_sub_arc2 'third part, if there is line, it is curved right or straight IF ir_1 OR ir_3 OR ir_5 THEN brick_f = 0: GOSUB mv_sub_b_short: GOSUB locate_line GOSUB mv_trn_r: GOSUB mv_trn_r: RETURN ENDIF 'PAUSE 2000 GOSUB mv_sub_arc2 'fourth part, if there is line, it is curved left IF ir_1 OR ir_3 OR ir_5 THEN brick_f = 0: GOSUB mv_sub_b_short: GOSUB locate_line RETURN ENDIF GOSUB mv_sub_arc2 'fourth part, if there is line, it is curved VERY left IF ir_1 OR ir_3 OR ir_5 THEN brick_f = 0: GOSUB mv_sub_b_short: GOSUB locate_line RETURN ENDIF RETURN mv_sub_r: p13=850: p12=850:pcount=22:GOSUB mv_sub 'right RETURN mv_sub_l: p13=650: p12=650:pcount=24:GOSUB mv_sub 'left RETURN mv_sub_l_half: p13=650: p12=650:pcount=12:GOSUB mv_sub 'left RETURN mv_sub_f: p13 = 850: p12=650:pcount=20:GOSUB mv_sub 'fwd RETURN mv_sub_f_short: p13 = 850: p12=650:pcount=8:GOSUB mv_sub 'fwd RETURN mv_sub_b_short: p13 = 650: p12=850:pcount=4:GOSUB mv_sub 'bkwd RETURN mv_trn_r: p13 = 850: p12=750:pcount=21:GOSUB mv_sub 'fwd RETURN mv_trn_l: p13 = 750: p12=650:pcount=21:GOSUB mv_sub 'fwd RETURN mv_sub_arc1: 'first part, no line check p13 = 772: p12=650:pcount=80:GOSUB mv_sub RETURN mv_sub_arc2: 'second part, doing line check p13 = 772: p12=650:pcount=60:GOSUB mv_sub2 '230 for big arc RETURN 'p13 = 815: p12=650:pcount=30:GOSUB mv_sub 'fwd 'p13=640: p12=640:pcount=21:GOSUB mv_sub 'left 'p13 = 815: p12=650:pcount=30:GOSUB mv_sub 'fwd 'p13=840: p12=840:pcount=20:GOSUB mv_sub 'right 'p13=675: p12=850:pcount=30:GOSUB mv_sub 'bkwd 'p13=640: p12=640:pcount=21:GOSUB mv_sub 'left 'p13=675: p12=850:pcount=30:GOSUB mv_sub 'bkwd 'p13=840: p12=840:pcount=20:GOSUB mv_sub 'right mv_sub: FOR i=1 TO pcount PULSOUT 13, p13 PULSOUT 12, p12 PAUSE 20 NEXT PAUSE 100 'for robot to stop before reversing servos RETURN mv_sub2: 'while looking for line FOR i=1 TO pcount PULSOUT 13, p13 PULSOUT 12, p12 PAUSE 20 GOSUB measure IF ir_1 OR ir_2 OR ir_3 OR ir_4 OR ir_5 THEN RETURN ENDIF NEXT PAUSE 100 'for robot to stop before reversing servos RETURN mv_right: PULSOUT 13, 750+speed: PULSOUT 12, 750+speed-speed-5 RETURN mv_right_more: PULSOUT 13, 750+speed: PULSOUT 12, 750+speed-(speed / 2)-5 RETURN mv_left: PULSOUT 13, 750-speed+speed-5: PULSOUT 12, 750-speed RETURN mv_left_more: PULSOUT 13, 750-speed+(speed / 2)-5: PULSOUT 12, 750-speed RETURN mv_fwd: 'PULSOUT 13, 750+speed-10: PULSOUT 12, 750-speed PULSOUT 13, 750+speed: PULSOUT 12, 750-speed RETURN mv_bk: 'PULSOUT 13, 750-speed+10: PULSOUT 12, 750+speed PULSOUT 13, 750-speed: PULSOUT 12, 750+speed RETURN locate_line: 'finding line, if it is lost for 20 pulses ~ 9cm 'look right then left p13=850: p12=850:pcount=7:GOSUB mv_sub 'right p13=850: p12=650:pcount=22:GOSUB mv_sub2 'fwd + looking for line IF ir_1 OR ir_2 OR ir_3 OR ir_4 OR ir_5 THEN: RETURN: ENDIF p13=650: p12=850:pcount=24:GOSUB mv_sub 'bkwd p13=650: p12=650:pcount=18:GOSUB mv_sub 'left p13=850: p12=650:pcount=22:GOSUB mv_sub2 'fwd + looking for line IF ir_1 OR ir_2 OR ir_3 OR ir_4 OR ir_5 THEN: RETURN: ENDIF p13=650: p12=850:pcount=24:GOSUB mv_sub 'bkwd p13=850: p12=850:pcount=7:GOSUB mv_sub 'right 'again and further PAUSE 200 'stop p13=850: p12=650:pcount=35:GOSUB mv_sub2 'fwd + looking for line IF ir_1 OR ir_2 OR ir_3 OR ir_4 OR ir_5 THEN: RETURN: ENDIF p13=650: p12=850:pcount=31:GOSUB mv_sub 'bkwd 'look left then right p13=850: p12=850:pcount=16:GOSUB mv_sub 'right p13=850: p12=650:pcount=30:GOSUB mv_sub2 'fwd + looking for line IF ir_1 OR ir_2 OR ir_3 OR ir_4 OR ir_5 THEN: RETURN: ENDIF p13=650: p12=850:pcount=32:GOSUB mv_sub 'bkwd p13=650: p12=650:pcount=30:GOSUB mv_sub 'left p13 = 850: p12=650:pcount=30:GOSUB mv_sub2 'fwd + looking for line IF ir_1 OR ir_2 OR ir_3 OR ir_4 OR ir_5 THEN: RETURN: ENDIF p13=650: p12=850:pcount=32:GOSUB mv_sub 'bkwd p13=850: p12=850:pcount=16:GOSUB mv_sub 'right 'it gets here, if there is no line after both tries 'FREQOUT 4,500, 2500: PAUSE 1000: FREQOUT 4,500, 2500 'STOP RETURN