TODOPIC

Mecatrónica => Robótica => Mensaje iniciado por: fran558 en 31 de Enero de 2006, 08:47:00

Título: ayuda con el codigo pbasic 2.5
Publicado por: fran558 en 31 de Enero de 2006, 08:47:00

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.
 

Título: RE: ayuda con el codigo pbasic 2.5
Publicado por: fran558 en 31 de Enero de 2006, 09:31:00

Para mas informacion adjunto unas fotos.
En el sensor derecho solo he activado el led derecho, y en el izquierdo el led izquierdo.


http://picserver.org/v/8U1XQN9NE89D
http://picserver.org/v/69H0WL53G621