'Programa ROB1 - para robo ADR-1 Andarilho 1 - Basic Stamp I ou Basic Step I
'Marcio Soares - 01/08/2001
'Funciona com PBasic Parallax ou TBasic Tato

symbol led    = 2		'led, quando 1 acende esquerdo
				'quando 0 acende direito 

symbol buzzer = 5		'buzzer

symbol sdir  = 7    	'servo direito
symbol sesq  = 6    	'servo esquerdo

symbol chave1 = 3		'chave direita
symbol chave2 = 4		'chave esquerda

symbol d1     = b3	'dado para servo direito
symbol d2     = b4	'dado para servo esquerdo
symbol i      = b2	'variavel para contador

let pins = 0		'faz pinos igual a zero
let dirs = %11100111	'saidas 4 e 3 entradas. 0,1,2,5,6 e 7 saidas
	
	low led

'loop principal
'analisa chaves, se nao tem nada anda para frente
'se chave precionada, gira para lado contrario
inicio:
	input pin3		'analisa chaves
	if pin3=0 then direita  'chave direira pressionada
	input pin4
	if pin4=0 then esquerda 'chave esquerda pressionada

        let d1=250		'nada pressionado, anda para frente
        let d2=50
	gosub anda		

	goto inicio		'faz eternamente...

'funcao da chave direita
'faz robo girar a esquerda
direita:
	high led	'acende led direito
	gosub inverte   'anda para traz durante 50ms
   
        let d1=0        'prepara para girar a esquerda
	let d2=255
        for i=1 to 30
	  gosub gira
        next i
        goto inicio

'funcao da chave esquerda
'faz robo girar a direita
esquerda:
	low led		 'acende led esquerdo
	gosub inverte    'anda para traz durante 50ms
       
        let d1=50        'prepara para girar a direita
	let d2=0
        for i=1 to 30
	  gosub gira
        next i
        goto inicio

'funcao para andar para traz comuns as
'funcoes DIREITA e ESQUERDA
inverte:
	for i=1 to 255		'toca buzzer...
	  pulsout buzzer,100
	next i
	let d1=50       'prepara para andar para traz
	let d2=255
	for i=1 to 50
          gosub gira
        next i
	return

'funcao para movimentar servos
'sem compensacao de velocidade
gira:	
	pulsout sesq,d2  'movimenta servos de acordo com direcao
	pulsout sdir,d1
        pause 20
	return

'funcao para movimentar para frente robo
'possui compensacao. Um dos servos eh mais rapido que o outro
'entao o mais lento movimenta-se 50% mais que o outro
anda:
	toggle led	 'pisca leds
	for i= 1 to 5		'movimenta servos juntos
	  pulsout sesq,d2
	  pulsout sdir,d1
	  pause 20
        next i

	for i=1 to 3		'movimenta apenas um servo
	  pulsout sdir,d1		'para compensar torcao no movimento
	  pause 20		'um dos servos eh mais rapido
        next i

	return