Boe Boy

Overview

Author(s):
Country: Greece Romania Russia
Date: 13.08.2009
Activity: Centrobot Robotic Summer School 2009
Location: Vienna, Austria
Hardware: LEGO Mindstorms NXT

Abstract

Our robot...

Project Files

The source code:

'{$STAMP BS2}
'{$PBASIC 2.5}
FREQOUT 4, 2000, 3000

' -----[ I/O Definitions ]-------------------------------------------------

Ping            PIN     14
qti VAR Nib
pulseCount VAR Byte


' -----[ Constants ]-------------------------------------------------------

#SELECT $STAMP
  #CASE BS2, BS2E
    Trigger     CON     5                       ' trigger pulse = 10 uS
    Scale       CON     $200                    ' raw x 2.00 = uS
  #CASE BS2SX, BS2P, BS2PX
    Trigger     CON     13
    Scale       CON     $0CD                    ' raw x 0.80 = uS
  #CASE BS2PE
    Trigger     CON     5
    Scale       CON     $1E1                    ' raw x 1.88 = uS
#ENDSELECT

RawToIn         CON     889                     ' 1 / 73.746 (with **)
RawToCm         CON     2257                    ' 1 / 29.034 (with **)

IsHigh          CON     1                       ' for PULSOUT
IsLow           CON     0


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

rawDist         VAR     Word                    ' raw measurement
inches          VAR     Word
cm              VAR     Word

' -----[ Program Code ]----------------------------------------------------

DO

'------[ Reading ] --------------------------------------------------------
 HIGH 3
 HIGH 15
 HIGH 8
 PAUSE 1
 qti.BIT0 = IN2
 qti.BIT1 = IN9
 qti.BIT2 = IN7
'-------------------------------------------------------------------------

'-----[ Decision of following the line ] ----------------------------------
SELECT qti

CASE %000 ' Forward
  PULSOUT 12, 850
  PULSOUT 13, 650
CASE %010 ' Forward
  PULSOUT 12, 850
  PULSOUT 13, 650
CASE %011 ' Pivot right
  PULSOUT 12, 750
  PULSOUT 13, 650
CASE %001 ' Rotate right
  PULSOUT 12, 850
  PULSOUT 13, 850
CASE %110 ' Pivot Left
  PULSOUT 12, 750
  PULSOUT 13, 650
CASE %100 ' Rotate Left
  PULSOUT 12, 650
  PULSOUT 13, 650
CASE %101 ' Forward
  PULSOUT 12, 850
  PULSOUT 13, 650

ENDSELECT
'--------------------------------------------------------------------------

'------[ Reading ] --------------------------------------------------------

PAUSE 50
GOSUB Get_Sonar                             ' get sensor value
inches = rawDist ** RawToIn                 ' convert to inches
cm = rawDist ** RawToCm                     ' convert to centimeters
'-------------------------------------------------------------------------

'------[ Avoiding the obstacle ] -----------------------------------------
IF ( cm < 10 AND cm > 7 ) THEN
  FREQOUT 4, 2000, 2500, 3000
  GOSUB Turn_Left
  GOSUB Stopp
  GOSUB Forward_Pulse
  GOSUB Stopp
  GOSUB Turn_Right
  GOSUB Stopp
  GOSUB Forward_Pulse
  GOSUB Stopp
  GOSUB Turn_Right
ENDIF
'------------------------------------------------------------------------

LOOP
'------------------------------------------------------------------------


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


Get_Sonar:
' This subroutine triggers the Ping sonar sensor and measures
' the echo pulse.  The raw value from the sensor is converted to
' microseconds based on the Stamp module in use.  This value is
' divided by two to remove the return trip -- the result value is
' the distance from the sensor to the target in microseconds.
  Ping = IsLow                                  ' make trigger 0-1-0
  PULSOUT Ping, Trigger                         ' activate sensor
  PULSIN  Ping, IsHigh, rawDist                 ' measure echo pulse
  rawDist = rawDist */ Scale                    ' convert to uS
  rawDist = rawDist / 2                         ' remove return trip

RETURN

Forward_Pulse: ' Send a single forward pulse.
FOR pulseCount = 0 TO 240
  PULSOUT 12,850
  PULSOUT 13,650
NEXT

FOR pulseCount = 0 TO 80
  PULSOUT 12,850
  PULSOUT 13,650
NEXT

RETURN

Turn_Left: ' Left turn, about 90-degrees.
FOR pulseCount = 0 TO 140
  PULSOUT 12, 650
  PULSOUT 13, 650
NEXT

RETURN

Turn_Right: ' Right turn, about 90-degrees.
FOR pulseCount = 0 TO 100
  PULSOUT 12, 850
  PULSOUT 13, 850
NEXT

RETURN

Rotate: ' Rotating about 180-degrees
FOR pulseCount = 0 TO 230
  PULSOUT 12, 850
  PULSOUT 13, 850
NEXT

RETURN

Stopp: '  Send a single stop pulse.
  PULSOUT 12, 750
  PULSOUT 13, 750

RETURN

Go back to the List of the projects

• NJM