'------------------------------------------------------------------
'Alex.bs2
'
'This program reads 3 RC channels (pulses) into the Basic Stamp and
'uses them to control the drive motor, steering motor, and head motor.
'



'------------------------------------------------------------------
'Variable Declarations
'
direction var byte		'Direction. '1'=fwd, '0'=rev.
motor var byte			'motor #. '1' - '4'
speed var byte			'speed variable. '0' - '6'
distance var word			'motor distance/position variable.
char var byte			'get chars from coprocessor.

temp var word			'temp for pulsin.

rec1 var byte			'input bytes from reciever unit.
rec2 var byte
rec3 var byte
rec4 var byte
rec5 var byte

prerec1 var byte
prerec2 var byte
prerec3 var byte
prerec4 var byte

'
'------------------------------------------------------------------
'Constant Declarations
'
net con 8				'coprocessor network pin.
baud con 396			'coprocessor network baud rate.
speaker con 9			'speaker pin.

rc1 con 2				'rc reciever input pins.
rc2 con 3
rc3 con 4
rc4 con 5
rc5 con 6


main:

	gosub getrc			'get rc reciever inputs.

	if abs(rec1-prerec1) < 2 then main1
	gosub dorec1		'turn robot steering.
main1:
	if abs(rec2-prerec2) < 2 then main2
	gosub dorec2		'move robot.
main2:
	if abs(rec3-prerec3) < 2 then main3
	gosub dorec3		'not used.
main3:
	if abs(rec4-prerec4) < 2 then main4
	gosub dorec4		'turn camera.
main4:

	gosub dorec5		'siren.

	prerec1=rec1
	prerec2=rec2
	prerec3=rec3
	prerec4=rec4

	goto main			'done.



'------------------------------------------------------------------
'get the rc reciever inputs.
'
getrc:

	pulsin rc1,1,temp		'get reciever inputs.
	rec1=((temp-404)/2)
	if temp < 914 then getrc1
	rec1=255
getrc1:
	if temp<>0 then getrc2
	rec1=0
getrc2:
	
	pulsin rc3,1,temp
	rec3=((temp-404)/2)
	if temp < 914 then getrc3
	rec3=255
getrc3:
	if temp<>0 then getrc4
	rec3=0
getrc4:

	pulsin rc5,1,temp
	rec5=((temp-370)/2)
	if temp < 914 then getrc5
	rec5=255
getrc5:
	if temp<>0 then getrc6
	rec5=0
getrc6:

	pulsin rc2,1,temp
	rec2=((temp-404)/2)
	if temp < 914 then getrc7
	rec2=255
getrc7:
	if temp<>0 then getrc8
	rec2=0
getrc8:

	pulsin rc4,1,temp
	rec4=((temp-404)/2)
	if temp < 914 then getrc9
	rec4=255
getrc9:
	if temp<>0 then getrc10
	rec4=0
getrc10:

return


'------------------------------------------------------------------
dorec1:
	if rec1<>0 then dorec10
	return			'check if signal recieved.
dorec10:

	motor="1"
	distance=rec1
	gosub rcservo

	return


'------------------------------------------------------------------
dorec2:
	if rec2<>0 then dorec20
	return			'check if signal recieved.
dorec20:

	if rec2<215 then dorec2a
	speed="A"
	direction="1"
	goto dorec2z
dorec2a:
	if rec2<210 then dorec2b
	speed="9"
	direction="1"
	goto dorec2z
dorec2b:
	if rec2<205 then dorec2c
	speed="8"
	direction="1"
	goto dorec2z
dorec2c:
	if rec2<200 then dorec2d
	speed="7"
	direction="1"
	goto dorec2z
dorec2d:
	if rec2<195 then dorec2e
	speed="6"
	direction="1"
	goto dorec2z
dorec2e:
	if rec2<190 then dorec2f
	speed="5"
	direction="1"
	goto dorec2z
dorec2f:
	if rec2<185 then dorec2g
	speed="4"
	direction="1"
	goto dorec2z
dorec2g:
	if rec2<180 then dorec2h
	speed="3"
	direction="1"
	goto dorec2z
dorec2h:
	if rec2<175 then dorec2i
	speed="2"
	direction="1"
	goto dorec2z
dorec2i:
	if rec2<170 then dorec2j
	speed="1"
	direction="1"
	goto dorec2z
dorec2j:
	if rec2<85 then dorec2k
	speed="0"
	direction="1"
	goto dorec2z
dorec2k:
	if rec2<80 then dorec2l
	speed="1"
	direction="0"
	goto dorec2z
dorec2l:
	if rec2<75 then dorec2m
	speed="2"
	direction="0"
	goto dorec2z
dorec2m:
	if rec2<70 then dorec2n
	speed="3"
	direction="0"
	goto dorec2z
dorec2n:
	if rec2<65 then dorec2o
	speed="4"
	direction="0"
	goto dorec2z
dorec2o:
	if rec2<60 then dorec2p
	speed="5"
	direction="0"
	goto dorec2z
dorec2p:
	if rec2<55 then dorec2q
	speed="6"
	direction="0"
	goto dorec2z
dorec2q:
	if rec2<50 then dorec2r
	speed="7"
	direction="0"
	goto dorec2z
dorec2r:
	if rec2<45 then dorec2s
	speed="8"
	direction="0"
	goto dorec2z
dorec2s:
	if rec2<40 then dorec2t
	speed="9"
	direction="0"
	goto dorec2z
dorec2t:
	speed="A"
	direction="0"

dorec2z:
	distance=65535
	gosub drive

	return


'------------------------------------------------------------------
dorec3:
	return


'------------------------------------------------------------------
dorec4:
	if rec4<>0 then dorec40
	return			'check if signal recieved.
dorec40:

	motor="2"
	distance=rec4
	gosub rcservo

	return


'------------------------------------------------------------------
dorec5:
	if rec5<>0 then dorec50
	return			'check if signal recieved.
dorec50:

	if rec5 > 50 then dorec5d
	freqout speaker,150,2000
dorec5d:

	return


'------------------------------------------------------------------
'Control RC servo motors.
'mtr (Motor #) '1', '2', '3', '4'.   '1'=steering motor.
'dis (distance/position)  = '00' to 'FF', where '00' = relax
'
rcservo:
	serout net,baud,["!1R",motor,hex2 distance]  'Send command to co.
	serin net,baud,1,rcservo,[char]			'Get "A".
	return


'------------------------------------------------------------------
'Control the drive motor without waiting until distance it traveled.
'Direction '1'(forward) or '0'(backwards)
'speed '0','1','2','3','4','5','6'  (0=off,6=full speed)
'distance '0000' to 'FFFF' distance to travel
'
drive:
	speed = speed | "0"
	direction = direction | "0"
	serout net,baud,["!1M1", direction, speed, hex4 distance]
	serin net,baud,1,drive,[char]			'Get "A" back.
	return