' IF debug is on or not
gtg con 1

VS_IN con 19
VL_IN con 18

csrv con 6 ' Center Servo
lsrv con 7 ' Left leg Servo
rsrv con 8 ' Right Leg Servo

az_pan con 4
elv_pan con 5

lird con 12
rird con 13
in_ird con 14

csrd var byte ' Current locations of center, left, right
lsrd var byte
rsrd var byte

csrv_mid 	con 128 ' Cserv middle
lsrv_mid 	con 128
rsrv_mid 	con 128

az_mid		con 145
az_pan_inc	con 100
elv_low		con	50
elv_fwd		con 125
elv_high	con	250

mspd 		con 210 ' Mid speed

lift 		con 120   ' Lift
stride	 	con 65 ' Stride

scanrange 	var word                 ' A/D result variable
floating 	var float                      ' Floating point math result storage
val 		var byte                             ' Table conversion result storage
xw			var byte
xx 			var byte = 0
xy			var byte = 0
xz			var byte = 0
rand		var word
rand1		var word
action 		var byte
dist 		var byte
pan 		var byte(3)
prev_dist	var byte
stop_walk	var bit = 1
happy		var nib = 0
l_movement  var nib(5)
og			var	nib
count1		var word
count2		var nib

vs			var word
vl			var word
vs1			var float
vl1			var float

rand = 189139813921
rand = random rand
rand = random rand
rand = random rand
rand = random rand

scantable bytetable     80,80,80,80,80,80,80,80,80,78, |
                        76,74,72,70,68,66,64,62,60,59, |
                        58,57,55,53,52,51,50,49,48,47, |
                        45,43,42,41,40,39,38,37,35,33, |
                        32,31,30,30,29,29,28,28,27,27, |
                        26,26,26,25,25,25,24,24,24,23, |
                        23,22,22,21,21,20,20,20,19,19, |
                        18,18,18,17,17,16,16,16,15,15, |
                        15,14,14,13,13,13,12,12,11,11, |

let tma=%00011001 ;set Timer Mode Register

oninterrupt timeraint, tmr_a_isr

'#####################
'# Startup Procedure #
'#####################

Startup
	enablehservo %0000000111110000,1000,2000,20
pause 2000

#IFDEF gtg
	gosub Center_Legs
	Sound 9,[100\5800,50\2600,80\4000]
#ENDIF

enable timeraint ' Enable Ground/Forward Checks
pause 3000
' Main Part of the Program

Main
	happy = 0
	
  Do
  	gosub chk_lvl
  	'	pan(0) is forward down
  	'	pan(1) is left up,
  	'	pan(2) is center up
  	'	pan(3) if right up
  	
	if pan(0) <= 22 then
		stop_walk = 0
		action = 1
		og	= 0
	else 									' This is for when we are picked up, do shit, until.
		stop_walk = 1
		action = 0
		happy = 1

		if og < 4 then
			Sound 9, [150\9000,100\8700]
			pause 1000
			hservo [lsrv\lsrv_mid\100]
			hservo [rsrv\rsrv_mid\100]
		endif
		
		hservo [az_pan\az_mid\200]
		hservo [elv_pan\elv_low\200]
		
			gosub get_range
			if (dist <= 25) then
				happy = 0
				action = 1
				stop_walk = 0
				hservo [elv_pan\elv_fwd\170]
				pause 3500
					Sound 9, [100\6800,50\4980]
				GOTO Main
			endif
		og = og + 1
		pause 500
	endif
	
	if pan(1) <= 43 then
		action = action + 4
		stop_walk = 1
	endif
	if pan(2) <= 35 then
		action = action + 5
		stop_walk = 1
	endif
	if pan(3) <= 43 then
		action = action + 7
		stop_walk = 1
	endif
	
	if action = 1 then
		if count2 > 6 THEN
			if pan(1) > 60 then
				action = action + 7
				stop_walk = 1
				count2 = 0
			elseif pan(3) > 60
				action = action + 4
				stop_walk = 1
				count2 = 0
			endif
		endif
	else
		count2 = 0
	endif
	
	
	'	1  = good to go :D
	'	5	= Object to the left 	- turn right
	'	6	= Object to the Center	- Backup
	'	8	= Object to the right	- Turn left
	'	10	= Object to the center/left	- Backup, turn right
	'	11	= Object is center/right	- Backup, turn left
	'	12	= Object to left/right		- Backup, rand[left/right]
	'	17	= Surrounded				- Backup
	
	'l_movement data(0) (last turn) 3 = right, 4 = left
	'l_movement data(1). 1 = fwd, 2 = reverse, 3 = right, 4 = left
	
	l_movement(1) = l_movement(0)	' Assign old movement to current one.
		
	IF action = 1 THEN
		gosub fstride
		l_movement(0) = 1
		stop_walk = 0
		count2 = count2 + 1
		xz = xz + 1
	ELSEIF action = 5
		if l_movement(0) = 4 THEN
			gosub tleft
			l_movement(0) = 4
			l_movement(1) = 4
		else
			gosub tright
			l_movement(0) = 3
			l_movement(1) = 3
		endif
		pause 300
		stop_walk = 0
	ELSEIF action = 6
		gosub rstride
		gosub rand_rturn
		pause 300
		stop_walk = 0
	ELSEIF action = 8
		if l_movement(0) = 3 THEN
			gosub tright
			l_movement(0) = 3
			l_movement(1) = 3
		else
			gosub tleft
			l_movement(0) = 4
			l_movement(1) = 4
		endif

		pause 300
		stop_walk = 0
	ELSEIF action = 10
		'gosub rstride
		
		gosub tright
		pause 300
		stop_walk = 0
	ELSEIF action = 11
		'gosub rstride
		
		gosub tleft
		pause 300
		stop_walk = 0
	ELSEIF action = 12
	
		'gosub rstride
		
		gosub rand_rturn
		pause 300
		stop_walk = 0
	ELSEIF action = 17
		Sound 9,[25\3800]
			rand = random rand
			rand = (rand // 22) + 1
		if rand >= 13 then
			Sound 9,[50\6000]
			gosub tright
			pause 500
			gosub tright
			pause 500
			gosub tright
			pause 500
			gosub tright
			happy = 1
			xz = 0
			gosub Center_Legs
			goto rest_mode
		else
			gosub rstride
			gosub rand_rturn
			pause 500
			stop_walk = 0
		endif
	ELSE
		Sound 9,[100\9000]
		pause 1000
		stop_walk = 1
	ENDIF
	action = 0
	stop_walk = 1
	
	if xz >= 50 then
		happy = 1
		xz = 0
		gosub Center_Legs
		goto rest_mode
	endif
	
	While happy
		
Goto Main

'###################################
'#			End of Loop			   #
'###################################

rest_mode
	Sound 9, [50\700,40\6000,10\5000]
	'disable timeraint ' Disable looking
	xw = 1
	pause 200
	count1 = 5
	
	Do
		gosub chk_lvl
		

	 	xw = xw + 1
	 	rand = (rand // 21) + 1
	 	
	 	count1 = count1 + rand
	 	
		if count1 > 10000 THEN
			Sound 9, [120\2700,80\5000,50\8000]
			pause 1000
			Sound 9, [10\2700,40\3000,50\4000]
			happy = 0
		endif
	 	
	 	if happy = 0 then
	 		goto endrest
	 	endif
	 			
	 		if xw = 20 THEN
	 			
				if (rand1 < 14) then
					hservo [az_pan\az_mid-az_pan_inc\100]
				elseif (rand1 < 27)
					hservo [az_pan\az_mid+az_pan_inc\120]
				elseif (rand1 < 40)
					hservo [az_pan\az_mid\160]
				endif

	 			
	 			if (rand > 17) THEN
	 				hservo [elv_pan\elv_high\170]
	 			else
	 				hservo [elv_pan\elv_fwd\130]
	 			endif
	 			
	 			xw = 1

	 		endif
	 		
		FOR count2 = 1 TO 5

	 			gosub get_range
	
				rand1 = random rand * 2
				rand1 = (rand1 // 41) + 1 ' This is for another random timer. It should end up different due
										 ' to the fact that its called at a different time, from our timer.
				pause 250
	 	NEXT
	 	
		gosub get_range
		' Okay, so this is clear to myself. We check the distance. Here.
		
		if (prev_dist+10) < dist OR (prev_dist-10) > dist THEN
			happy = 0
		endif
		
		prev_dist = dist
	 	
	 While happy = 1
	
endrest:
	
	Sound 9, [50\8700,60\3000,50\8000]
	pause 2500
goto Main

rand_rturn:														' When avoiding forward objects
'##########################
'# New Method of Movement #
'##########################

if l_movement(1) = 3 THEN
	gosub tright
elseif l_movement(1) = 4
	gosub tleft
else
	rand = random rand
	rand = (rand // 21) + 1

	if pan(0) <= 22 then
		if rand > 0 AND rand < 8 THEN
			gosub tright
			l_movement(0) = 3
		elseif rand > 6 AND rand < 15
			gosub tleft
			l_movement(0) = 4
		else
			gosub rstride
		endif
	endif
		rand = 6795 // (rand + scanrange)
endif

return

' Check For Ground
tmr_a_isr:
	if happy = 1 THEN
		resume
	endif
	
	rand = random rand
	gosub get_range
												' Panic Action		
if xy = 0 then
		hservo [elv_pan\elv_fwd\170]
		hservo [az_pan\az_mid-az_pan_inc\240]
		pan(0) = dist
		
		if dist <= 22 then
			stop_walk = 0
		else
			stop_walk = 1
		endif

		xy = 1
elseif xy = 1
		hservo [az_pan\az_mid\240]
		pan(1) = dist
		if dist <= 38 then
			stop_walk = 1
		else
			stop_walk = 0
		endif
		xy = 2
elseif xy = 2
		hservo [az_pan\az_mid+az_pan_inc\240]
		pan(2) = dist
		if dist <= 35 then
			stop_walk = 1
		else
			stop_walk = 0
		endif
		xy = 3
elseif xy = 3
		xy = 0
		pan(3) = dist
		if dist <= 38 then
			stop_walk = 1
		else
			stop_walk = 0
		endif
		hservo [az_pan\az_mid\200]
		hservo [elv_pan\elv_low\200]
endif
	
resume

Center_Legs:
	hservo [az_pan\az_mid\50]		' Move servos to resting spot
	hservo [elv_pan\elv_fwd\255]
	
	hservo [csrv\csrv_mid-lift\200]
	pause 500
	hservo [lsrv\lsrv_mid\200]
	pause 500
	hservo [csrv\csrv_mid+lift\200]
	pause 500
	hservo [rsrv\rsrv_mid\200]
	pause 500
	hservo [csrv\csrv_mid\200]
return
                        
                        
'######################
'#     Ranging Info   #
'######################
get_range:
			pause 200
			ADIN 0, scanrange
            floating = tofloat scanrange / 5.12                 ' Limit value to <200 values

            val = scantable(toint floating)                            ' Convert A/D to measurement

            if scanrange > 512 then
            	dist = 0
            else
            	dist = val
            endif
return

'#######################
'#   Walking Motions   #
'#######################

fstride:
	if stop_walk = 1 THEN
		csrd = csrv_mid
		gosub setsrv_mspd
		goto Main
	endif

	csrd = csrv_mid - lift
	lsrd = lsrv_mid + stride
	rsrd = rsrv_mid + stride
	
	if stop_walk = 1 THEN
		csrd = csrv_mid
		gosub setsrv_mspd
		goto Main
	endif
	gosub setsrv_mspd
	pause 180
	
	
	csrd = csrv_mid - lift
	lsrd = lsrv_mid - stride
	rsrd = rsrv_mid - stride
	
	if stop_walk = 1 THEN
		csrd = csrv_mid
		gosub setsrv_mspd
		goto Main
	endif
	gosub setsrv_mspd
	pause 180
	
	csrd = csrv_mid + lift
	lsrd = lsrv_mid - stride
	rsrd = rsrv_mid - stride
	
	if stop_walk = 1 THEN
		csrd = csrv_mid
		gosub setsrv_mspd
		goto Main
	endif
	
	gosub setsrv_mspd
	pause 180
	
	csrd = csrv_mid + lift
	lsrd = lsrv_mid + stride
	rsrd = rsrv_mid + stride
	
	if stop_walk = 1 THEN
		csrd = csrv_mid
		gosub setsrv_mspd
		goto Main
	endif
	
	gosub setsrv_mspd
	pause 180
	
	csrd = csrv_mid
	gosub setsrv_mspd
	pause 180
return

rstride:
	
	csrd = csrv_mid + lift
	lsrd = lsrv_mid + stride
	rsrd = rsrv_mid + stride
	gosub setsrv_mspd
	pause 200
	
	csrd = csrv_mid + lift
	lsrd = lsrv_mid - stride
	rsrd = rsrv_mid - stride
	gosub setsrv_mspd
	pause 200
	
	csrd = csrv_mid - lift
	lsrd = lsrv_mid - stride
	rsrd = rsrv_mid - stride
	gosub setsrv_mspd
	pause 200
	
	csrd = csrv_mid - lift
	lsrd = lsrv_mid + stride
	rsrd = rsrv_mid + stride
	gosub setsrv_mspd
	pause 200
	
	csrd = csrv_mid
	gosub setsrv_mspd
	pause 200
return

tright:

      lsrd=lsrv_mid-stride
      rsrd=rsrv_mid+stride
      csrd=csrv_mid
      gosub setsrv_mspd
'      pause 1000

      lsrd=lsrv_mid-stride	'Left side up.
      rsrd=rsrv_mid+stride
      csrd=csrv_mid+lift
      gosub setsrv_mspd
      pause 200

      lsrd=lsrv_mid+stride	'Stride one.
      rsrd=rsrv_mid-stride
      csrd=csrv_mid+lift
      gosub setsrv_mspd
      pause 200

      lsrd=lsrv_mid+stride	'Right side up.
      rsrd=rsrv_mid-stride
      csrd=csrv_mid-lift
      gosub setsrv_mspd
      pause 200

      lsrd=lsrv_mid-stride	'Stride two.
      rsrd=rsrv_mid+stride
      csrd=csrv_mid-lift
      gosub setsrv_mspd
      pause 200
   
	csrd = csrv_mid
	gosub setsrv_mspd
	pause 200
return

tleft:
   for xx = 1 to 4
      lsrd=lsrv_mid+stride
      rsrd=rsrv_mid-stride
      csrd=csrv_mid
      gosub setsrv_mspd
   next      
      lsrd=lsrv_mid+stride	'Left side up.
      rsrd=rsrv_mid-stride
      csrd=csrv_mid+lift
      gosub setsrv_mspd
      pause 200

      lsrd=lsrv_mid-stride	'Stride one.
      rsrd=rsrv_mid+stride
      csrd=csrv_mid+lift
      gosub setsrv_mspd
      pause 200

      lsrd=lsrv_mid-stride	'Right side up.
      rsrd=rsrv_mid+stride
      csrd=csrv_mid-lift
      gosub setsrv_mspd
      pause 200

      lsrd=lsrv_mid+stride	'Stride two.
      rsrd=rsrv_mid-stride
      csrd=csrv_mid-lift
      gosub setsrv_mspd
      pause 200
       

	csrd = csrv_mid
	gosub setsrv_mspd
	pause 200
return

'1.45 is what we read when battery is dead
'So, here is the formula.
' vs1 = vs / 1000
' vs2 = (vs1 * 5) * 4
'1.45 / 5

chk_lvl:

rebeep

ADIN VS_IN, vs
ADIN VL_IN, vl

vs1 = tofloat vs / 1023
vs1 = vs1 * 5
vs1 = vs1 * 4
vl1 = tofloat vl / 1023
vl1 = vl1 * 5
vl1 = vl1 * 4
	debug ["vs[", REAL vs1, "]  vl[", REAL vl1, "]  "]
if vs1 < 5.2 OR vl1 < 5.2 THEN
	Sound 9,[100\8000,100\3000,100\5000]
	goto rebeep
endif

return

setsrv_mspd:
	hservo [csrv\csrd\255]
	hservo [rsrv\lsrd\mspd]
	hservo [lsrv\rsrd\mspd]
return