Hola a todos:
Me acabo de aficionar a la robotica y me he contruido un diseño con dos
servos uno para cada rueda (der e izq) y una tercera rueda loca.
El mecanismo esta controlado con una Basic Stamp 2e montada en la
Next Step v2.0
Con el codigo que pongo mas abajo quiero que al detectar objetos por
la derecha gire a la izquierda y vicebersa. Cuando dertecta obstaculo al
frente que retroceda y mire a un lado y otro para segidamente tome una
de estas cuatro decisiones:
obstaculo a la derecha ->gira a la izquierda y avanza, y vicebersa;
obstaculo a ambos lados ->retrocede y vuelve a mirar a ambos lados;
sin obstaculos -> gira 180º y avanza
La cosa es que cuando avanza detecta bien los obstaculos y gira bien
la cosa cambia cuando anda hacia atras y comprueba a un lado y otro
ya que me hace cosas muy rarar, generalmente me da un pqueño giro y
avanza independientemente si detecta obstaculos o no. He rebisado
una y otra vez el codigo y no se que anda mal. Si algin le puede hechar
un vistazo lo agradeceria.
" {$STAMP BS2e}
" {$PBASIC 2.5}
"Conexiones
"Servo izq p0
"Servo der p1
"Sensor infrarrojos izq p12,p13 y p14(outup)
"Sensor infrarrojos der p9, p10 y p11(outup)
"Zumbador p8
"SHARP p15
contador VAR Word
ledI VAR Bit
ledD VAR Bit
mirarizq VAR Bit
mirarder VAR Bit
inicio:
PAUSE 2000
DO
HIGH 6
PAUSE 1000
LOW 6
HIGH 7
PAUSE 1000
LOW 7
IF IN4=0 THEN arrancar
IF IN5=0 THEN parar
LOOP
arrancar:
PAUSE 2000
avanzar:
PULSOUT 0,650
PULSOUT 1,850
GOTO detectar
giroder:
PULSOUT 0,650
PULSOUT 1,650
GOTO detectar
giroizq:
PULSOUT 0,850
PULSOUT 1,850
GOTO detectar
retroceder:
FOR contador=1 TO 80
PULSOUT 0,850
PULSOUT 1,650
PAUSE 20
NEXT
GOTO mirar
accion:
IF mirarizq=0 AND mirarder=0 THEN retroceder
IF mirarizq=0 AND mirarder=1 THEN giroderecha
IF mirarizq=1 AND mirarder=0 THEN giroizquierda
IF mirarizq=1 AND mirarder=1 THEN giro180
mirar:
PAUSE 2000
GOSUB giroizq90
mirarizq=1
PAUSE 2000
GOSUB detectarB
IF ledI=0 THEN
mirarizq=0
ENDIF
IF ledD=0 THEN
mirarizq=0
ENDIF
GOSUB giroder90
PAUSE 2000
GOSUB giroder90
mirarder=1
PAUSE 2000
GOSUB detectarB
IF ledI=0 THEN
mirarder=0
ENDIF
IF ledD=0 THEN
mirarder=0
ENDIF
GOSUB giroizq90
PAUSE 2000
GOTO accion
giroizquierda:
HIGH 8
PAUSE 500
LOW 8
HIGH 8
PAUSE 500
LOW 8
GOSUB giroizq90
GOTO avanzar
giroderecha:
HIGH 8
PAUSE 500
LOW 8
GOSUB giroder90
GOTO avanzar
giroizq90:
FOR contador=1 TO 30
PULSOUT 0,850
PULSOUT 1,850
PAUSE 20
NEXT
RETURN
giroder90:
FOR contador=1 TO 30
PULSOUT 0,650
PULSOUT 1,650
PAUSE 20
NEXT
RETURN
giro180:
FOR contador=1 TO 60
PULSOUT 0,650
PULSOUT 1,650
NEXT
GOTO detectar
detectar:
HIGH 12
PAUSE 10
ledI=IN14
LOW 12
HIGH 10
PAUSE 10
ledD=IN11
LOW 10
IF IN4=0 THEN inicio
IF IN5=0 THEN parar
IF ledI=1 AND ledD=1 THEN avanzar
IF ledI=0 AND ledD=1 THEN giroder
IF ledI=1 AND ledD=0 THEN giroizq
IF ledI=0 AND ledD=0 THEN retroceder
detectarB:
HIGH 12
ledI=IN14
PAUSE 10
LOW 12
HIGH 10
ledD=IN11
PAUSE 10
LOW 10
RETURN
parar:
END
Hasta la proxima.
