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