' Constants for my Boe-bot
left_servo	con	15		' Pin for left servo
right_servo	con	3		' Pin for right servo
left_cent	con	740		' zero motion pulse for left servo
right_cent	con	750		' zero motion pulse for right servo
LED		con	0		' Pin for my LED
speaker	con	12		' Pin for my speaker


' Constants for turning 90 degrees
turnspeed	con	97
turntime	con	25

' Constants for forward and backward motion
speed		con	100
lspeed	con	speed
rspeed	con	-speed
runtime	con	200

' Various local variables
t1		var	word
t2		var	word
t3		var	word


' pins used
output	left_servo
output	right_servo
output	LED
output	speaker


' 10 second countdown before motion starts
for t1 = 1 to 10
  freqout speaker,500,1000
  high LED
  pause 500
  low LED
next

goto main

' -----------------------------
' My subroutines
' -----------------------------
'--- goforward
gfrx			var	t1	
goforward:
for gfrx=1 to runtime
  pulsout left_servo,left_cent+lspeed
  pulsout right_servo,right_cent+rspeed
  pause 20
next
return

'--- gobackward
gbwx			var	t1
gobackward:
for gbwx=1 to runtime
  pulsout left_servo,left_cent-lspeed
  pulsout right_servo,right_cent-rspeed
  pause 20
next
return

'--- beepbackward
bbwperiod		con	50
bbwhalfper		con	bbwperiod/2
bbwfreq		con	2000
bbwx			var	t1
bbwj			var	t2.highbyte
beepbackward:
bbwj = 0
for bbwx=1 to runtime
  pulsout left_servo,left_cent-lspeed
  pulsout right_servo,right_cent-rspeed

  if bbwj > 0 then bbw_gt_0
    high LED
    freqout speaker,20,bbwfreq
    goto bbw_end

  bbw_gt_0:
  if bbwj >= bbwhalfper then bbw_ge_hlfper
    freqout speaker,20,bbwfreq
    goto bbw_end

  bbw_ge_hlfper:
  if bbwj > bbwhalfper then bbw_gt_hlfper
    low LED
    pause 20
    goto bbw_end

  bbw_gt_hlfper:
    pause 20

  bbw_end:

  bbwj = bbwj + 1
  if bbwj <> bbwperiod then bbw_dont_clear
    bbwj = 0
  bbw_dont_clear:

next
low LED
return 


'--- turnright 90 degrees
trrx			var	t1
turnright:
for trrx=1 to turntime
  pulsout left_servo,left_cent+turnspeed
  pulsout right_servo,right_cent+turnspeed
  pause 20
next
return

'--- turnleft 90 degrees
trnperiod		con	10
trnbeepon		con	7
trlx			var	t1
trlj			var	t2.highbyte
trlj=0
turnleft:
for trlx=1 to turntime
  pulsout left_servo,left_cent-turnspeed
  pulsout right_servo,right_cent-turnspeed

  if trlj > 0 then trl_gt_0
    high LED
    pause 20
    goto trl_end

  trl_gt_0:
  if trlj >= trnbeepon then trl_ge_beepon
    pause 20
    goto trl_end

  trl_ge_beepon:
  if trlj > trnbeepon then trl_gt_beepon
    low LED
    pause 20
    goto trl_end

  trl_gt_beepon:
    pause 20

  trl_end:

  trlj = trlj + 1
  if trlj <> trnperiod then trl_dont_clear
    trlj = 0

  trl_dont_clear:
next
low LED
return

' ---------- prime beeper
primes	data	2,3,5,7,11,13,17,23,29,31,0
prpntr	var	t1.highbyte
prdata	var	t1.lowbyte
prj		var	t2.highbyte
primebeep:
  prpntr = primes

  pb_loop:
    read prpntr, prdata
    if prdata = 0 then pb_lpend
    for prj=1 to prdata
      freqout speaker,600,1500
      pause 400
    next
    pause 2000
    prpntr = prpntr + 1
    read prpntr, prdata
  goto pb_loop
  
  pb_lpend:
  return

' ---------------
' ---- ce3kbeep -- Close Encounters of the 3rd Kind beep
cl3kndsong	data	3,2,1,5,3,1,4,3,1,5,6,1,1,1,4,0,0,0
cl3kndptr	var	t1.highbyte
cl3knddat	var	t1.lowbyte
cl3freq	var	t2
cl3dur	var	t3
cl3basefreq	con	900
cl3basedur  con   800
ce3kbeep:
  cl3kndptr = cl3kndsong

  cl3k_loop:
    read cl3kndptr,cl3knddat		' Read numerator
    if cl3knddat = 0 then cl3k_lpend
    cl3freq = cl3basefreq*cl3knddat
    cl3kndptr = cl3kndptr + 1

    read cl3kndptr,cl3knddat		' Read denominator
    cl3freq = cl3freq / cl3knddat
    cl3kndptr = cl3kndptr + 1

    read cl3kndptr,cl3knddat		' Read duration
    cl3dur = cl3basedur * cl3knddat
    cl3kndptr = cl3kndptr + 1

    freqout speaker,cl3dur,cl3freq

  goto cl3k_loop

  cl3k_lpend:
    return
 
' ---------------
' the MAIN routine
maini		var	byte
main:

gosub ce3kbeep
pause 1000

for maini= 1 to 3
  gosub goforward
  pause 1000
  gosub turnright
  pause 1000
next

gosub goforward
pause 1000

pause 500
for maini= 1 to 4
  gosub beepbackward
  pause 1000
  gosub turnleft
  pause 1000
next

gosub primebeep