'{$STAMP BS2}
init:

main_loop_count con 2
tolerance con 2 'feet
further_feet con 2
closer_feet con 2
button_inc con 1

'speed calc
yint con 79
slope con 299

distance_corr_mult con 101 
'distance_corr_mult = (3600/5280)*slope/corr_div
' for (3600/5280)*299/2=101.93

'throttle control
vacuum_pulse con 17000 'sucks on pulse
release_pulse con 48535 ' relases on no pulse

'laser
timeout con 1000 'ms
laser_power_on_time con 500 'ms
laser_init_time con 3100 'ms

'end constants

'set pin directions
'	dirs=%0000011101000001
	dirs= $0741


'throttle
	release1 var out9
	release1_pin con 9
	release2 var out8
	vacuum var out10
	vacuum_pin con 10
	vacuum=$0
	release2=$0
	release1=$0

'switch
	blue var in4
	green var in3
	yellow var in2

	button_count var nib
	button_count=0	

'lcd
	clrLCD con 	12	' Clear entire LCD screen. 
	posCmd con 	16	' Position cursor. 
	ESC    con 	27	' Escape code for graphics instructions. 
	light_on con 14
	light_off con 15

	n9600  con	$4054	' Baudmode for 9600 bps, inverted. 
	first_line con 64
	second_line con 20+64
	third_line con 40+64
	fourth_line con 60+64

	lcd_distance con second_line + 15
	lcd_distance_desired con third_line + 15
	lcd_speed con second_line+8
	lcd_speed_desired con third_line+8
	lcd_pin con 0

'  Team Laser Chaser
'Actual: XXMPH  XXXFT
'Target: XXMPH  XXXFT
'   Status Messages


'distance reader
	raw_distance var byte(8)
	distance_READ var byte
	distance_read=250

	distance_con_pin con 6
	distance_rx_pin con 7

	distance_period var byte


'clutch
	clutch var in13

'brake
	brake var in14

'tach
	tach_pin con 12
	tach_speed var word

'goals
	distance_goal var word
	distance_goal=250

	active var bit
	active=0

'calc_tach
desired_tach var word
distance_read_old var byte

' Night Rider!
 light_count var word
 light_count=0

'main loop
counter var nib

start:
' startup screen
low distance_con_pin
pause laser_power_on_time
serout lcd_pin,n9600,[clrLCD,light_off] 	' Clear screen and reset font. ,ESC,"F",64
serout lcd_pin,n9600,["Team Laser Chaser",cr,"Cruise Control",cr,"V 3.1b Initializing!"]	' Print a message. 
high distance_con_pin
pause laser_init_time
low distance_con_pin

serout 0,n9600,[clrLCD] 	' Clear screen  
serout 0,n9600,["  Team Laser Chaser",cr,"Actual: XXMPH XXXFT",cr,"Target: XXMPH  XXXFT",cr,"    Ready!"]	' Print a message. 
'  Team Laser Chaser
'Actual: XXMPH  XXXFT
'Target: XXMPH  XXXFT
'   Status Messages

'serout 0,n9600,["Target  Distance:",cr,"Target  Velocity:",cr,"Desired Distance:",cr,"    Ready!"]	' Print a message. 
'Target  Distance:XX
'Target  Velocity:XX
'Desired Distance:XX
'Status

loop: 'main loop
	'loop so we don't wait on laser
	for counter=0 to main_loop_count

		'check brake n clutch
		if clutch=1 and brake=1 and blue=0 or active=0 then safe

		'check clutch
		if clutch=1 then clutch_ok
		gosub clutch_on

		'check brake
clutch_ok:	if brake=1 then brake_ok
		gosub brake_on

		'check if off
brake_ok:	if blue=0 then safe
		gosub off

safe:		'check buttons
		
'old		if green=1 and yellow=0 then set_press
		if not(green=1 and yellow=0) then set_not_pressed
		gosub set_press


' old		if green=0 and yellow=0 then resume_press
set_not_pressed: if green=1 or yellow=1 then resume_not_pressed		
		gosub resume_press

resume_not_pressed:gosub show_speed

	gosub show_ready		

	gosub night_rider
	next ' done with main loop

		gosub get_dist
		gosub calc_tach 

adjust_throttle:

	'dumb version
	if active=1 and abs(distance_read - distance_goal) =< tolerance then hold
		if distance_read > distance_goal and active=1 then faster
		     if distance_read < distance_goal and active=1 then slower
		
	'smarter
'	if active=1 and abs(tach_read - tach_goal) < tolerance then hold
'		if tach_read > tach_goal and active=1 then faster
'		     if tach_read < tach_goal and active=1 then slower


goto loop


set_press: 'set button pressed decide set or closer
	serout 0,n9600,[poscmd,fourth_line,"Set Pressed!   "]	' Print a message. 

	if active=0 then set

' ok not set then closer
closer: 'move car closer in by closer_feet
	button_count=button_count+button_inc
	if button_count <> 0 then loop
	
	distance_goal=distance_goal -1

	serout lcd_pin,n9600,[poscmd,lcd_distance_desired,dec distance_goal]	' Print a message. 
		if distance_goal > 99 then closer_done
		serout lcd_pin,n9600,[" "]	' Print a message. 
		if distance_goal > 9 then closer_done
		serout lcd_pin,n9600,[" "]	' Print a message. 

closer_done:
return

set: 'set distnace_goal to distance_read
	serout lcd_pin,n9600,[poscmd,fourth_line,"       Set!        "]	' Print a message. 
	distance_goal=distance_read
	active=1
	serout 0,n9600,[poscmd,fourth_line,"Activated!    "]	' Print a message. 
	button_count=0

	serout lcd_pin,n9600,[poscmd,lcd_distance_desired,dec distance_goal ]	' Print a message. 
		if distance_goal > 99 then set_done
		serout lcd_pin,n9600,[" "]	' Print a message. 
		if distance_goal > 9 then set_done
		serout lcd_pin,n9600,[" "]	' Print a message. 

set_done: return




'RESUME

resume_press: 'set button pressed decide set or closer
	serout 0,n9600,[poscmd,fourth_line,"Resume Pressed!"]	' Print a message. 
	if active=0 then resume
' if not resuem the further

further: 'move car further in by further_feet
	button_count=button_count+button_inc
	if button_count <> 0 then loop

	distance_goal=distance_goal + further_feet


	serout lcd_pin,n9600,[poscmd,lcd_distance_desired,dec distance_goal]	' Print a message. 
		if distance_goal > 99 then further_done
		serout lcd_pin,n9600,[" "]	' Print a message. 
		if distance_goal > 9 then further_done
		serout lcd_pin,n9600,[" "]	' Print a message. 

further_done: return
resume: active=1
	button_count=0
	serout 0,n9600,[poscmd,fourth_line,"      Activated!    ",light_on]	' Print a message. 
return

brake_on:
	serout 0,n9600,[poscmd,fourth_line,"Brake Pressed!"]	' Print a message. 
	active=0
	vacuum=$0
	release1=$0
	release2=$0
	serout lcd_pin,n9600,[poscmd,lcd_distance_desired,"   "]	' Print a message. 

return

clutch_on:
	serout 0,n9600,[poscmd,fourth_line,"Clutch Pressed!"]	' Print a message. 
	active=0	
	vacuum=$0
	release1=$0
	release2=$0
	serout lcd_pin,n9600,[poscmd,lcd_distance_desired,"   "]	' Print a message. 

return

faster: 
pulsout vacuum_pin,vacuum_pulse
'vacuum=$1
	release1=$1
	release2=$1
goto loop

slower: vacuum=$0
	pulsout release1_pin,release_pulse
	'release1=$0
	release2=$1
goto loop

hold: vacuum=$0
	release1=$1
	release2=$1
goto loop

show_speed: 

' may not need
'raw_distance(0)=current speed
'raw_distance(1)=desried speedor leading car speed

	raw_distance(0)=yint-tach_speed/slope
	raw_distance(1)=yint-desired_tach/slope

	serout 0,n9600,[poscmd,lcd_speed,dec raw_distance(0)]	' Print a message. 
	if tach_speed > 99 then distance_done
	serout 0,n9600,[" "]	' Print a message. 
	if tach_speed > 9 then distance_done
	serout 0,n9600,[" "]	' Print a message. 

'now show desired
	serout 0,n9600,[poscmd,lcd_speed_desired,dec raw_distance(1)]	' Print a message. 
	if tach_speed > 99 then distance_done
	serout 0,n9600,[" "]	' Print a message. 
	if tach_speed > 9 then distance_done
	serout 0,n9600,[" "]	' Print a message. 
	
return

get_dist: 
	distance_read_old=distance_read
	distance_read=0 'reset distance_read to safe initial

	raw_distance(8)=0
	serin distance_rx_pin,16468,timeout,bad_laser,[str raw_distance\8]
'	serin distance_rx_pin,16468,[str raw_distance\8]
	
	DEBUG "reading ",raw_distance(0),raw_distance(1),raw_distance(2),raw_distance(3),raw_distance(4),raw_distance(5),raw_distance(6),raw_distance(7),cr

	'look for period
	for distance_period = 4 to 8
		if raw_distance(distance_period) = 46 then found
	next

	found:	
'debug "found ",raw_distance(distance_period), "at ",dec distance_period,cr


' decimal	distance_read= raw_distance(distance_period+2) -48
'decimal	distance_read= distance_read + ((raw_distance(distance_period+1)-48) *10)
'	debug "calc ", dec distance_read,cr
	
	if raw_distance(distance_period -1) = "," then distance_read_complete
		distance_read= distance_read +  ((raw_distance(distance_period-1)-48))

	if distance_period = 4 then distance_read_complete
	if raw_distance(distance_period -2) = "," then distance_read_complete
		distance_read= distance_read +  ((raw_distance(distance_period-2)-48) *10)

	if distance_period < 4 then distance_read_complete
	if raw_distance(distance_period -3) = "," then distance_read_complete
		distance_read= distance_read +  ((raw_distance(distance_period-3)-48) *100)


distance_read_complete:	

' debug "big num ",dec distance_read,cr
	'fill in over old numbers
	show_distance:
		serout lcd_pin,n9600,[poscmd,lcd_distance,dec distance_read]	' Print a message. 
		if distance_read > 99 then distance_done
		serout lcd_pin,n9600,[" "]	' Print a message. 
		if distance_read > 9 then distance_done
		serout lcd_pin,n9600,[" "]	' Print a message. 

distance_done: return


off:
	serout 0,n9600,[poscmd,fourth_line,"Deactivated!   ",light_off]	' Print a message. 
	active=0	
	vacuum=$0
	release1=$0
	release2=$0
	serout lcd_pin,n9600,[poscmd,lcd_distance_desired,"   "]	' Print a message. 
return

calc_tach:
	pulsin tach_pin,0,tach_speed
	desired_tach=tach_speed-slope*(distance_read-distance_read_old) + (distance_goal-distance_read)*distance_corr_mult

	debug "Current tach=",dec tach_speed," Desired tach=",dec desired_tach,cr

	if distance_read_old <> 0 then calc_tach_ok
	' don't have a good read, so carry on how we are
	desired_tach=tach_speed

calc_tach_ok:
return

show_ready:
if active=1 or blue=1 then show_ready_return
serout 0,n9600,[poscmd,fourth_line,"Ready!        ",light_on]	' Print a message. 
show_ready_return: return

bad_laser:
	serout lcd_pin,n9600,[poscmd,fourth_line,"Laser Error!    "]	' Print a message. 
	active=0	
	vacuum=$0
	release1=$0
	release2=$0
goto loop

night_rider:
	serout lcd_pin,n9600,[poscmd,first_line,"                    "]	' Print a message. 
	
	if light_count < 20 then night_rider_normal
		serout lcd_pin,n9600,[poscmd,first_line + (39 - light_count) ,134]	' Print a message. 
	goto night_rider_counter_inc 'aka skip the normal printing

	night_rider_normal:serout lcd_pin,n9600,[poscmd,first_line + light_count,134]	' Print a message. 

	night_rider_counter_inc:light_count=light_count +1
	if light_count < 41 then night_rider_return
		light_count=0

night_rider_return: return