'Programa para controle do RM-1 v 2.0
'Márcio José Soares - 30/07/2002
'Revista Mecatrônica Fácil
'programa p/ Basic Step 1

Symbol pos      = b0	'guarda posição do servo
Symbol nrServo  = b1	'guarda número do servo

Symbol pbase  = b2	'posição da base
Symbol pbraco = b3	'posição do braço
Symbol pgarra = b4	'posição da garra

Symbol aviso = b5

Symbol base  = 7	'nr servo base
Symbol braco = 6	'nr servo braço
Symbol garra = 5	'nr servo garra

Symbol maximo = 200	'posição máxima
Symbol minimo = 100	'posição mínima

pbase = 150		'inicia variáveis
pbraco = 150
pgarra = 190

dirs = %11101111

loop:
	aviso = 0
	pulsout base, pbase	'ativa servos
	pulsout braco, pbraco
	pulsout garra, pgarra
	
	input 4	'aguarda dado
	if pin4 = 1 then recebe
	pause 20
	if aviso = 0 then loop

'dado recebido, ok
avisa:
	serout 0,N2400,("1")
	goto loop

'recebe dado	
recebe:	
	serout 0,N2400,("P")
	serin 4,N2400,("BM1"),#nrServo,#pos

'testa dados
	if pos < minimo then erro
	if pos > maximo then erro

'testa servo valido	
	if nrServo = base then mvbase
	if nrServo = braco then mvbraco
	if nrServo = garra then mvgarra

'dado com erro
erro:
	serout 0,N2400,("0")
	goto loop

'muda valor da base
mvbase:
	aviso = 1
	pbase = pos
	goto avisa

'muda valor do braço
mvbraco:
	aviso = 1
	pbraco = pos
	goto avisa

'muda valor da garra
mvgarra:
	aviso = 1
	pgarra = pos
	goto avisa