' {$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