Two Rabbits & Two Hats

Overview

Author(s):
Grzegorz Marzencki
Alessandro Colombo
Vladimir Kotev
Country:

Poland
Italy
Bulgaria
Date: 13.08.2009
Activity: Centrobot Robotic Summer School 2009
Location: Vienna, Austria
Hardware: Boe Bot

Abstract

Linefollower with 3 sensores, avoiding obsticle, having mechanical claws (gripper) and using them to move objects.

Project Files

Presentation

' {$STAMP BS2}
' {$PBASIC 2.5}
' P10: line sensor
' P14: line sensor
' P12: line sensor
' P4: front green led
' P3: Front IR LED
' P5: Front IR Receiver

' -----[ Title ]--------------------------------------------------------------
' Robotics with the Boe-Bot - RoamingWithIr.bs2

DEBUG "Program Running!"
' -----[ Variables ]----------------------------------------------------------
irDetectLeft VAR Bit
irDetectFront VAR Bit
pulseCount VAR Byte
flag VAR Bit
' -----[ Initialization ]-----------------------------------------------------

qti VAR Nib
counter VAR Word

' -----[ Main Routine ]-------------------------------------------------------


flag = 0
DO
  GOSUB read_line
  DEBUG BIN3 qti, CR
  'DEBUG qti.BIT0, CR
  'DEBUG qti.BIT1, CR
  'DEBUG qti.BIT2, CR
  FREQOUT 3, 1, 39000
  irDetectFront = IN5

 ' GOSUB Forward_Pulse
  'break:


    DEBUG CRSRXY, 2, 6, BIN1 irDetectFront
  IF  (irDetectFront = 0) THEN
    GOSUB  Back_up
    GOSUB Turn_Right ' Back up & U-turn
    'FREQOUT 4, 2000, 3000 ' Signal program start/reset.
    PAUSE 20
    FOR pulseCount = 0 TO 50
    GOSUB Forward_Pulse
    NEXT

    GOSUB Turn_Left
    FOR pulseCount = 0 TO 100
    GOSUB Forward_Pulse
    NEXT
    GOSUB Turn_Left
     PAUSE 20
   ' FOR pulseCount = 0 TO 50
   DO
   GOSUB read_line
     GOSUB Forward_Pulse
   ' NEXT
     IF (qti) THEN
        GOSUB T_right
        GOSUB T_right
        GOTO break
     ENDIF
   LOOP
  ENDIF
  break:

  '''''''''''''''

    SELECT qti
    CASE %000 ' Forward

    GOSUB Forward

    CASE %010 ' Forward

    GOSUB Forward


    CASE %001 '  right

    GOSUB T_right

    CASE %011 '  less right

    GOSUB L_right


    CASE %100 '  left

    GOSUB T_left

    CASE %110 '  less left

    GOSUB L_left


    CASE %111 ' grapping
        ' grap
    PAUSE 1000
    GOSUB Forward
    IF (flag = 0)  THEN
    DO
     GOSUB read_line
        IF (qti =%111) THEN
           'PAUSE 1000
           GOSUB Forward
        ENDIF
        IF (qti = %001) OR  (qti = %011)  THEN
        GOSUB L_left
        PAUSE 1000
        ENDIF
        IF (qti = %110) OR   (qti = %100) THEN
        GOSUB L_right
         PAUSE 1000
        ENDIF
        IF (qti = %000) THEN
           FOR pulseCount = 0 TO 12
               GOSUB Forward
           NEXT
           GOSUB grab
           FOR pulseCount = 0 TO 6
                GOSUB Back_up
           NEXT
           FOR pulseCount = 0 TO 6
                GOSUB Back_up
           NEXT
           FOR pulseCount = 0 TO 6
                GOSUB Back_up
           NEXT
           FOR pulseCount = 0 TO 6
                GOSUB Back_up
           NEXT
           GOSUB Turn_Back
           flag = 1
           GOTO grip_end
        ENDIF
    LOOP
    ENDIF
    IF (flag = 1)  THEN
DO
     GOSUB read_line
        IF (qti =%111) THEN
           'PAUSE 1000
           'GOSUB Forward
        ENDIF
        IF (qti = %001) OR  (qti = %011)  THEN
        GOSUB L_left
        PAUSE 1000
        ENDIF
        IF (qti = %110) OR   (qti = %100) THEN
        GOSUB L_right
         PAUSE 1000
        ENDIF
        IF (qti = %000) THEN
           FOR pulseCount = 0 TO 12
               GOSUB Forward
           NEXT
           GOSUB relese
           DO
             GOSUB   stop_pulse
           LOOP

        ENDIF
    LOOP
    ENDIF
   grip_end:




  ENDSELECT
LOOP
END




' -----[ Subroutines ]--------------------------------------------------------


stop_pulse:
   PULSOUT 13,759
   PULSOUT 12,750
grab:
FOR counter = 1 TO 100
   PULSOUT 15, 1000
   PAUSE 20
   NEXT
RETURN

relese:
FOR counter = 1 TO 100
   PULSOUT 15, 700
   PAUSE 20
   NEXT
RETURN

read_line:
  qti.BIT0 = IN8
  qti.BIT1 = IN14
  qti.BIT2 = IN10
RETURN

Forward_Pulse: ' Send a single forward pulse.
PULSOUT 13,850
PULSOUT 12,650
PAUSE 20
RETURN
Turn_Left: ' Left turn, about 90-degrees.
FOR pulseCount = 0 TO 23
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 20
NEXT
RETURN
Turn_Right:
FOR pulseCount = 0 TO 25 ' Right turn, about 90-degrees.
PULSOUT 13, 850
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN
Back_up:
FOR pulseCount = 0 TO 8
PULSOUT 13, 650
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN

Turn_Back:
FOR pulseCount = 0 TO 50 ' Right turn, about 180-degrees.
GOSUB read_line
PULSOUT 13, 850
PULSOUT 12, 850
PAUSE 20
IF (qti AND (pulseCount>40)) THEN
GOTO ex
ENDIF
NEXT
ex:
RETURN

Forward:
FOR counter = 1 TO 2
PULSOUT 13, 875
PULSOUT 12, 725
PAUSE 20
NEXT
RETURN

T_left:
FOR counter = 1 TO 8
PULSOUT 13, 675
PULSOUT 12, 675
PAUSE 20
NEXT
RETURN

L_left:
FOR counter = 1 TO 3
PULSOUT 13, 725
PULSOUT 12, 650
PAUSE 20
NEXT
RETURN



T_right:
FOR counter = 1 TO 8
PULSOUT 13, 875
PULSOUT 12, 875
PAUSE 20
NEXT
RETURN


L_right:
FOR counter = 1 TO 3
PULSOUT 13, 875
PULSOUT 12, 800
PAUSE 20
NEXT
RETURN

Go back to the List of the projects