'Source Code for Autonomous Bastard (1.0)
'For Basic Stamp 2.0

'Features:
'Servo control
'Radio interface for safety
'Infrared range-finding
'Photosensor for kill-saw-zone detection



'{$STAMP BS2}
'{$PBASIC 2.5}

'loop variables
x VAR Byte
y VAR Nib

Time  VAR  Word
servo1 VAR Word
servo2 VAR Word
weapon VAR Byte

photo VAR Word
photo_calib VAR Word

ir_freq VAR Word

ir_led VAR Nib
ir_detector VAR Nib

'sensor variables

ir_value VAR Byte(5)
ir_prev VAR Byte(5)


LOW 13          ' Initialize servo line startup values.
LOW 12

GOSUB get_photo_calib

FOR x = 1 TO 250
  PULSOUT 2, 15
NEXT

FOR x = 1 TO 250
  PULSOUT 2, 70
NEXT

FOR x = 1 TO 250
  PULSOUT 2, 500
NEXT

init:

weapon = 195

'GOTO skip_init

GOSUB safety_check

GOSUB esc_init

'wait for throttle back up before fight
GOSUB throttle_down_again_wait

GOSUB get_photo_calib

skip_init:

'run check_sensors once to fill values
GOSUB check_sensors

'fight!

'GOSUB go_forward
GOSUB go_forward
GOSUB forward_left
GOSUB forward_right

main:

'DEBUG "fighting!!!!"

GOSUB check_sensors

GOSUB maintain_esc

GOSUB fail_safe_check

GOSUB maintain_esc

GOSUB get_photo

GOSUB maintain_esc

GOSUB check_sensors

GOSUB maintain_esc

IF ir_value(0) < 25 AND ir_value(1) < 25 THEN GOSUB go_backward

IF ir_value(0) < 25 OR ir_value(1) < 25 THEN GOSUB back_little

IF ABS (ir_prev(4) - ir_value(4)) > 2 THEN GOSUB turn_around
IF ABS (ir_prev(0) - ir_value(0)) > 2 THEN GOSUB forward_right
IF ABS (ir_prev(1) - ir_value(1)) > 2 THEN GOSUB forward_left
IF ABS (ir_prev(2) - ir_value(2)) > 2 THEN GOSUB turn_right
IF ABS (ir_prev(3) - ir_value(3)) > 2 THEN GOSUB turn_left

FOR x= 0 TO 4
'  IF ir_prev(x) <>  ir_value(x) THEN DEBUG CLS, "not dancing" 'GOTO no_dance
  IF ABS (ir_prev(x) - ir_value(x)) > 2 THEN GOTO no_dance
NEXT

IF ir_prev(4) <>  ir_value(4) THEN GOSUB turn_around
IF ir_prev(0) <>  ir_value(0) THEN GOSUB forward_right
IF ir_prev(1) <>  ir_value(1) THEN GOSUB forward_left
IF ir_prev(2) <>  ir_value(2) THEN GOSUB turn_right
IF ir_prev(3) <>  ir_value(3) THEN GOSUB turn_left

FOR x= 0 TO 4
  IF ir_prev(x) <> ir_value(x) THEN GOTO no_dance
NEXT

DEBUG CLS, "dancing"

GOSUB dance

no_dance:

GOSUB maintain_esc

GOTO main

dance:

GOSUB fail_safe_check

GOSUB set_forward_right
FOR x = 1 TO 20
   GOSUB out
NEXT


GOSUB set_forward_left
FOR x = 1 TO 20
   GOSUB out
NEXT


GOSUB turn_right
FOR x = 1 TO 9
   GOSUB out
NEXT


GOSUB turn_left
FOR x = 1 TO 9
   GOSUB out
NEXT


RETURN



turn_around:
  GOSUB fail_safe_check

  GOSUB set_left

  FOR x = 1 TO 30
    GOSUB out
  NEXT
RETURN


go_forward:
  GOSUB fail_safe_check
  GOSUB set_forward
  FOR x = 1 TO 30
    GOSUB out
  NEXT
RETURN

forward_left:
  GOSUB fail_safe_check
  GOSUB set_forward_left
  FOR x = 1 TO 35
    GOSUB out
  NEXT
RETURN

forward_right:
  GOSUB fail_safe_check
  GOSUB set_forward_right
  FOR x = 1 TO 35
    GOSUB out
  NEXT
RETURN

turn_left:
  GOSUB fail_safe_check
  GOSUB set_left
  FOR x = 1 TO 15
    GOSUB out
  NEXT
RETURN

turn_right:
  GOSUB fail_safe_check
  GOSUB set_right
  FOR x = 1 TO 15
    GOSUB out
  NEXT
RETURN


go_backward:
'  GOSUB fail_safe_check
  DEBUG "backing up"
  GOSUB set_backward
  FOR x = 1 TO 85
    GOSUB out
  NEXT
RETURN



back_little:
'  GOSUB fail_safe_check
  GOSUB set_backward
  FOR x = 1 TO 20
    GOSUB out
  NEXT
RETURN



set_forward:
  servo1 = 1000
  servo2 = 500
RETURN


set_backward:
  servo1 = 500
  servo2 = 1000
RETURN

set_left:
  servo1 = 500
  servo2 = 500
RETURN

set_right:
  servo1 = 1000
  servo2 = 1000
RETURN


set_forward_left:
  servo1 = 780
  servo2 = 500
RETURN

set_forward_right:
  servo1 = 1000
  servo2 = 740
RETURN


out:

  PULSOUT 2, 500
  PULSOUT 13, servo1
  PULSOUT 12, servo2
  PULSOUT 14, weapon * 4
  PAUSE 20

RETURN



esc_init:

'warning led
FOR x = 1 TO 135
  PULSOUT 2, 500
NEXT

PAUSE 500

FOR x = 1 TO 135
  PULSOUT 2, 500
NEXT


FOR x = 1 TO 100
  PULSOUT 14, 510
  PULSOUT 2, 500
  PAUSE 20
NEXT

FOR x = 1 TO 250
  PULSOUT 14, 990
  PULSOUT 2, 500
  PAUSE 20
NEXT

FOR x = 1 TO 75
  PULSOUT 14, 510
  PULSOUT 2, 500
  PAUSE 20
NEXT

'little spin to show good

FOR x = 1 TO 25
  PULSOUT 14, 550
  PULSOUT 2, 500
  PAUSE 20
NEXT


RETURN


safety_check:


throttle_down_wait:
FOR x = 1 TO 5
  PULSIN 0, 1, Time
  DEBUG CLS, DEC ? Time
  DEBUG "Waiting For Down"
  IF Time > 620 OR time < 500 THEN throttle_down_wait

  DEBUG CLS, "Throttle Down"

NEXT

FOR x= 1 TO 5
  PULSOUT 2, 500
NEXT

throttle_up_wait:
FOR x = 1 TO 5
  PULSIN 0, 1, Time
  DEBUG CLS, DEC ? Time
  DEBUG "Waiting For Up"
  IF Time < 880 OR time > 1000 THEN throttle_up_wait

  DEBUG CLS, "Throttle Up"

NEXT

FOR x= 1 TO 5
  PULSOUT 2, 500
NEXT


throttle_down_again_wait:
FOR x= 1 TO 5
  PULSIN 0, 1, Time    ' Measure positive pulse.
  DEBUG CLS, DEC ? Time    ' Otherwise, display result.
  DEBUG "Waiting For Down"

  'keep esc hot
  FOR y = 1 TO 8
    PULSOUT 14, 510
    PAUSE 20
  NEXT

  IF Time > 620 OR time < 500 THEN throttle_down_again_wait  ' If 0, try again.

  DEBUG "Throttle Down"

NEXT

FOR x= 1 TO 5
  PULSOUT 2, 500
NEXT



throttle_up_wait2:
FOR x = 1 TO 5
  PULSIN 0, 1, Time
  DEBUG CLS, DEC ? Time
  DEBUG "Waiting For Up"

  'keep esc hot
  FOR y = 1 TO 8
    PULSOUT 14, 510
    PAUSE 20
  NEXT

  PULSOUT 2, 140

  IF Time < 880 OR time > 1000 THEN throttle_up_wait2

  DEBUG "Safety Pass"

NEXT


RETURN


fail_safe_check:

  PULSIN 0, 1, Time
  GOSUB maintain_esc
  IF Time < 850 OR time > 1090 THEN GOSUB fail_safe_check_again

RETURN


fail_safe_check_again:

FOR x= 1 TO 6
  PULSIN 0, 1, Time
  IF Time > 850 AND time < 1090 THEN GOTO fail_safe_ok
  GOSUB maintain_esc
  DEBUG "lost signal"
NEXT

GOSUB throttle_up_wait2

fail_safe_ok:

RETURN



check_sensors:

      FOR x = 0 TO 4

          PULSOUT 2, 500

          ir_prev(x) = ir_value(x)

          LOOKUP x,[6,7,5,9,4],IR_detector
          LOOKUP x,[1,10,10,15,11],IR_led

          FOR y = 0 TO 4

'            LOOKUP y,[21925,21925,21925,21760,21295],IR_freq
            LOOKUP y,[30510,30500,21925,21760,21295],IR_freq
'            LOOKUP y,[21295,21760,21925,30500,30510],IR_freq
'            LOOKUP y,[21295,21760,21925,30500,30510],IR_freq

              FREQOUT ir_led, 2, IR_freq
              ir_value.LOWBIT((x * 8) + y) = INS.LOWBIT(IR_detector)

          NEXT

      NEXT

'      DEBUG HOME, DEC5 ir_value(0), CR
'      DEBUG DEC5 ir_value(1), CR
'      DEBUG DEC5 ir_value(2), CR
'      DEBUG DEC5 ir_value(3), CR
'      DEBUG DEC5 ir_value(4), CR

'      DEBUG CLS, DEC5 ir_value(1), CR
'      DEBUG DEC5 ir_value.LOWBIT(8), CR
'      DEBUG DEC5 ir_value.LOWBIT(9), CR
'      DEBUG DEC5 ir_value.LOWBIT(10), CR
'      DEBUG DEC5 ir_value.LOWBIT(11), CR
'      DEBUG DEC5 ir_value.LOWBIT(12), CR



RETURN

maintain_esc:

      FOR y = 1 TO 10
            PULSOUT 14, weapon * 4
            PAUSE 20
      NEXT

RETURN


get_photo:

  HIGH 3            ' Set P3 to output-high.
  PAUSE 3            ' Pause for 3 ms.
  RCTIME 3,1,photo        ' Measure RC time on P3.

'  DEBUG CR
'    DEBUG DEC5 photo / 2
'    DEBUG CR
'  DEBUG DEC5 photo_calib / 3

  IF photo / 2 < photo_calib / 3 THEN DEBUG "killsaw detected"
  IF photo / 2 < photo_calib / 3 THEN GOSUB go_backward


RETURN


get_photo_calib:

  HIGH 3            ' Set P3 to output-high.
  PAUSE 3            ' Pause for 3 ms.
  RCTIME 3,1,photo        ' Measure RC time on P3.

  photo_calib = photo
  DEBUG DEC5 photo_calib

RETURN