BOE-BOT Enviromental Scan Programming

Tue, 30 Nov 1999 00:11:00 EST

The robotic platform will be placed dead center of the workspace. Then robot is then required to perform a 360 rotation to scan its entire environment and then proceed to locate and move towards the farthest object stopping 2 inches in front of the object. The robot must then use it current location as its origin and perform the exact same task as before. The robot should be able to differentiate the wall from objects and not recognize a segment of the wall as obstacle.

' {$STAMP BS2}
' {$PBASIC 2.5}

' -----[ Variables ]----------------------------------------------------------

time           VAR Word
DR1            VAR Word
DR2            VAR Word
DR3            VAR Word
DR4            VAR Word
counter        VAR Nib
int            VAR Word
Ping_Pin    CON   15
InConstant  CON   890
inDistance  VAR   Word
i           VAR  Nib
R1          CON  13
R2          CON  26
R3          CON  39
R4          CON  42
rot         VAR  Word

Main:

PAUSE 2000
GOSUB ir_direction1
PAUSE 1000
GOSUB ir_direction2
PAUSE 1000
GOSUB ir_direction3
PAUSE 1000
GOSUB ir_direction4
PAUSE 1000
GOSUB Check
'DEBUG CR, "DR1 = ", DR1
'DEBUG CR, "DR2 = ", DR2
GOSUB Check2
END

ir_direction1:
DR1 = 0
PULSOUT 15,5
  PULSIN 15,1, DR1
  DEBUG HOME
  DR1 = DR1 ** 890

  DEBUG CR, "Distance1 = ", DEC4 DR1, "   in"
  'DEBUG CR, "DR1 = ", DR1
  PAUSE 1000
RETURN

ir_direction2:

FOR counter = 0 TO 13       ' LEFT ABOUT 90 DEGREES
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 200
NEXT
DR2 = 0
PULSOUT 15,5
  PULSIN 15,1, DR2
  DEBUG HOME
  DR2 = DR2 ** 890

  DEBUG CR, "Distance2 = ", DEC4 DR2, "   in"
  'DEBUG CR, "DR2 = ", DR2
  PAUSE 1000

RETURN
ir_direction3:

FOR counter = 0 TO  13       ' LEFT ABOUT 90 DEGREES
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 200
NEXT
DR2 = 0
PULSOUT 15,5
  PULSIN 15,1, DR3
  DEBUG HOME
  DR3 = DR3 ** 890

  DEBUG CR, "Distance3 = ", DEC4 DR3, "   in"
  'DEBUG CR, "DR3 = ", DR3
  PAUSE 1000
RETURN
ir_direction4:

FOR counter = 0 TO 13       ' LEFT ABOUT 90 DEGREES
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 200
NEXT
DR2 = 0
PULSOUT 15,5
  PULSIN 15,1, DR4
  DEBUG HOME
  DR4 = DR4 ** 890

  DEBUG CR, "Distance4 = ", DEC4 DR4, "   in"
  'DEBUG CR, "DR2 = ", DR2
  PAUSE 1000

RETURN

'_________________________________________________________________'

Check:
PAUSE 2000
IF (DR1 > DR2) AND (DR1 > DR3) AND (DR1 > DR4) THEN
    PAUSE 2000

    DEBUG "this is DR1"
    PAUSE 2000
    'rot = 13
    PAUSE 2000
    FOR counter = 0 TO  13       ' LEFT ABOUT 90 DEGREES
    PULSOUT 13, 650
    PULSOUT 12, 650
    PAUSE 200
    NEXT
    PAUSE 2000

    'GOSUB Check2

ELSEIF(DR2 > DR1) AND (DR2 > DR3) AND (DR2 > DR4) THEN
   'FOR counter = 0 TO 23       ' LEFT ABOUT 90 DEGREES
    'PULSOUT 13, 650
    'PULSOUT 12, 650
    'PAUSE 200
    'NEXT
    'GOSUB Check2
    DEBUG "this is DR2"
    PAUSE 2000
    'rot = 26
    PAUSE 2000
    FOR counter = 0 TO  26       ' LEFT ABOUT 90 DEGREES
    PULSOUT 13, 650
    PULSOUT 12, 650
    PAUSE 200
    NEXT
    'GOSUB Check2

ELSEIF(DR3 > DR1) AND (DR3 > DR2) AND (DR3 > DR4) THEN
    'PAUSE 2000
    'FOR counter = 0 TO 45       ' LEFT ABOUT 90 DEGREES
    'PULSOUT 13, 650
    'PULSOUT 12, 650
    'PAUSE 200
    'NEXT
    DEBUG "this is DR3"
    PAUSE 2000
    'rot = 39
    FOR counter = 0 TO  39       ' LEFT ABOUT 90 DEGREES
    PULSOUT 13, 650
    PULSOUT 12, 650
    PAUSE 200
    NEXT
    'GOSUB Check2

    PAUSE 2000

ENDIF
    PAUSE 2000
    'GOSUB Check2
    DEBUG "this is DR4"
    PAUSE 2000
    'DEBUG " niether"
    PAUSE 2000
    'GOSUB Check2

RETURN

Check2:
'FREQOUT 4, 2000, 3000
counter = 0

DO UNTIL (int < 4)

    GOSUB Readpulse
    PAUSE 2000
    'GOSUB forward
    GOSUB forward
    PAUSE 2000
   LOOP
PAUSE 1000

Readpulse:

'FREQOUT 4, 2000, 3000                        ' Signal program start/reset

PULSOUT Ping_Pin, 5              ' Send short pulse to Ping
  PULSIN Ping_Pin, 1, int         ' Wait for echo
  int = inConstant ** int  ' Convert to inches
  DEBUG CR, DEC int         ' Display result
  PAUSE 200         ' Short delay until next read
  RETURN

forward:

FOR counter =  1 TO 2

  PULSOUT 13, 850
  PULSOUT 12,650
  PAUSE 20

NEXT

RETURN

RoboRealm Forum

BOE-BOT Enviromental Scan Programming