' =========================================================================
'
'   File.......
'   Purpose....
'   Author..... Micaiah and Daniel McGlothin
'   Started.... 28-JUN-2005
'
'   {$STAMP BS2}
'   {$PBASIC 2.5}
'
' =========================================================================


' -----[ Program Description ]---------------------------------------------
' Right-Hand Wall Following
' Sensors:           Left Sonar, Parallax PING)))
'                    Right Sonar, Devantech SRF04
'                    Front IR Range Detector, Sharp GP2D02
' Motor Controller:  HVW Tech L298 Motor Control Kit
' Development Board: Basic Stamp Activity Board
' Processor:         Basic Stamp 2
'
' Algorithm:         Based on information from
'                       http://www.micromouseinfo.com/introduction/wallfollow.html
'                    and from
'                       http://coecsl.ece.uinc.edu/ge423/spring04/group10/files/lil_robot.c
'
' Test Environment:  Based on information from
'                       http://www.swallow.co.uk/umouse/miniwall.htm
'
'

' -----[ Revision History ]------------------------------------------------


' -----[ Compilation Directive Definitions ]-------------------------------------------------

#DEFINE SIMMETRY_LCD = 0       ' 0==no LCD attached; 1==LCD attached
#DEFINE DEBUG_OUTPUT = 0       ' 0==no output;       1==output

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

'
SimmetryOut       PIN     15

'motor outputs
LeftFwd           PIN     8
LeftRev           PIN     9
RightFwd          PIN     10
RightRev          PIN     11

'sensor I/O
LeftSonar         PIN     6   ' a PING))) module

RightSonar_Echo   PIN     4   ' a Devantech SRF04 module
RightSonar_Trig   PIN     5

FrontIR_data      PIN     2   ' a Sharp GP2D02 module
FrontIR_clock     PIN     3


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

IsHigh          CON     1     ' utility High & Low
IsLow           CON     0

TRUE            CON     1     ' utility Boolean constants
FALSE           CON     0

SonarMax        CON     3     ' number of Sonar readings to average

' Devantech SRF04 and Parallax PING))) 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     ' trigger pulse = 10 uS
    Scale       CON     $0CD   ' raw x 0.80 = uS
  #CASE BS2PE
    Trigger     CON     5      ' trigger pulse = 10 uS
    Scale       CON     $1E1   ' raw x 1.88 = uS
#ENDSELECT
RawToIn         CON     889    ' 1 / 73.746 (with **)
RawToCm         CON     2257   ' 1 / 29.034 (with **)

'serial communication constants
#SELECT $STAMP
  #CASE BS2, BS2E, BS2PE
    T1200       CON     813
    T2400       CON     396
    T4800       CON     188
    T9600       CON     84
    T19K2       CON     32
    TMidi       CON     12
    T38K4       CON     6
  #CASE BS2SX, BS2P
    T1200       CON     2063
    T2400       CON     1021
    T4800       CON     500
    T9600       CON     240
    T19K2       CON     110
    TMidi       CON     60
    T38K4       CON     45
  #CASE BS2PX
    T1200       CON     3313
    T2400       CON     1646
    T4800       CON     813
    T9600       CON     396
    T19K2       CON     188
    TMidi       CON     108
    T38K4       CON     84
#ENDSELECT
SevenBit        CON     $2000
Inverted        CON     $4000
Open            CON     $8000
Baud            CON     T9600


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

OpeningOnRight     VAR Bit
OpeningAhead       VAR Bit
OpeningOnLeft      VAR Bit

rawSonar1       VAR     Word     ' raw measurement for sonars
rawSonar2       VAR     Word     ' raw measurement for sonars

rawIR           VAR     Byte     ' raw measurement for IR Detector
cmIR            VAR     Byte
inIR            VAR     Byte

counter         VAR     Byte

F               VAR     Byte
R               VAR     Word
L               VAR     Word

' -----[ EEPROM Data ]-----------------------------------------------------


' -----[ Initialization ]--------------------------------------------------

Reset:
  DEBUG CLS, "MazeRunner starting...", CR
  #IF SIMMETRY_LCD = 1 #THEN
  GOSUB InitSimmetry
  #ENDIF
  #IF DEBUG_OUTPUT = 1 #THEN
  GOSUB InitDebug
  #ENDIF
  GOSUB MotorsOff
  LeftSonar = IsLow
  RightSonar_Trig = IsLow
  RightSonar_Echo = IsLow
  INPUT FrontIR_data
  HIGH FrontIR_clock
  PAUSE 2500

Testing:
  'GOTO PingTest
  'GOTO DevantechSRF04Test
  'GOTO SharpGP2D02Test
  'GOTO MotorTest


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

Main:
  DO
    GOSUB CheckFrontSensor
    GOSUB CheckRightSensor
    GOSUB CheckLeftSensor
    #IF SIMMETRY_LCD = 1 #THEN
    GOSUB UpdateSimmetry
    #ENDIF
    #IF DEBUG_OUTPUT = 1 #THEN
    GOSUB UpdateDebug
    #ENDIF
    GOSUB RightWallFollowDecisions
    GOSUB MoveForwardHalfCell
  LOOP

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

CheckFrontSensor:
  ' a Sharp GP2D02 module, range 4cm to 80cm
  '   Clock == detector pin 4, yellow wire
  '   Data  == detector pin 2, green  wire
  '            (returns sensed distance in cm)

  'get sensor value
  LOW FrontIR_clock       ' turn detector ON
  DO
    'wait until detector signals it has a sample
  LOOP UNTIL FrontIR_data = IsHigh
  rawIR = 0
  SHIFTIN FrontIR_data, FrontIR_clock, MSBPOST, [rawIR]
  HIGH FrontIR_clock       ' turn detector OFF
  PAUSE 2                  ' let detector reset

  'convert value to centimeters
  'conversion table derived from experimentation after
  'discovering that the output documented as non-linear in:
  '  HVW Technologies DIRRS.PDF Rev 1.4, page 3,
  '  found at http://www.pc-mod.com/robo/plans/dirrs.pdf
  'and
  '  http://www.mcselec.com/an16.htm which references
  '  a chapter from Claus Kuehnel's BASIC Stamp book
  LOOKDOWN rawIR, >[236, 228, 222, 215, 195, 171, 159, 154, 142, 132, 121, 109, 100,  95,  92,  89], rawIR
  LOOKUP   rawIR,  [  7,   8,   9,  10,  12,  14,  16,  18,  20,  25,  30,  40,  50,  60,  70,  80], cmIR
  LOOKUP   rawIR,  [  2,   3,   3,   4,   5,   6,   7,   7,   8,  10,  12,  15,  20,  23,  27,  31], inIR

  'interpret sensor value
  IF cmIR > 25 THEN
    OpeningAhead = TRUE
  ELSE
    OpeningAhead = FALSE
  ENDIF

  RETURN

' -------------------------------

CheckRightSensor:
  ' a Devantech SRF04 module, range 3cm to 3m

  'get sensor value
  RightSonar_Trig = IsLow
  RightSonar_Echo = IsLow
  PULSOUT RightSonar_Trig, Trigger
  OUTPUT  RightSonar_Trig
  RCTIME  RightSonar_Echo, IsHigh, rawSonar1
  rawSonar1 = rawSonar1 */ Scale   ' adjust for microprocessor model
  rawSonar1 = rawSonar1 / 2        ' remove return trip
  rawSonar1 = rawSonar1 ** RawToCm ' convert to centimeters

  'interpret sensor value
  IF rawSonar1 > 15 THEN
    OpeningOnRight = TRUE
  ELSE
    OpeningOnRight = FALSE
  ENDIF
  RETURN


' -------------------------------

CheckLeftSensor:
  ' a Parallax PING))) module, range 3cm to 3m

  'get sensor value
  LeftSonar = IsLow
  PULSOUT LeftSonar, Trigger
  PULSIN  LeftSonar, IsHigh, rawSonar2
  rawSonar2 = rawSonar2 */ Scale   ' adjust for microprocessor model
  rawSonar2 = rawSonar2 / 2        ' remove return trip
  rawSonar2 = rawSonar2 ** RawToCm ' convert to centimeters

  'interpret sensor value
  IF rawSonar2 > 15 THEN
    OpeningOnLeft = TRUE
  ELSE
    OpeningOnLeft = FALSE
  ENDIF
  RETURN


' -------------------------------

MoveForwardOneCell:
  'assume 18 inch cell
  GOSUB MotorsForward
  PAUSE 800
  GOSUB MotorsOff
  RETURN


' -------------------------------

MoveForwardHalfCell:
  'assume 18 inch cell
  GOSUB MotorsForward
  PAUSE 400
  GOSUB MotorsOff
  RETURN


' -------------------------------

RightWallFollowDecisions:
  IF OpeningOnRight THEN
    #IF SIMMETRY_LCD = 1 #THEN
    SEROUT SimmetryOut, Baud, ["Tm11,Turn Right  ", CR]
    PAUSE 10
    #ENDIF
    #IF DEBUG_OUTPUT = 1 #THEN
    GOSUB ReportDebug_TurnRight
    #ENDIF
    GOSUB MotorsSpinRight_090
  ELSEIF OpeningAhead THEN
    #IF SIMMETRY_LCD = 1 #THEN
    SEROUT SimmetryOut, Baud, ["Tm11,Go Forward  ", CR]
    PAUSE 10
    #ENDIF
    #IF DEBUG_OUTPUT = 1 #THEN
    GOSUB ReportDebug_MoveForward
    #ENDIF
    'do nothing, just continue straight ahead
  ELSEIF OpeningOnLeft THEN
    #IF SIMMETRY_LCD = 1 #THEN
    SEROUT SimmetryOut, Baud, ["Tm11,Turn Left   ", CR]
    PAUSE 10
    #ENDIF
    #IF DEBUG_OUTPUT = 1 #THEN
    GOSUB ReportDebug_TurnLeft
    #ENDIF
    GOSUB MotorsSpinLeft_090
  ELSE
    #IF SIMMETRY_LCD = 1 #THEN
    SEROUT SimmetryOut, Baud, ["Tm11,Turn Around ", CR]
    PAUSE 10
    #ENDIF
    #IF DEBUG_OUTPUT = 1 #THEN
    GOSUB ReportDebug_TurnAround
    #ENDIF
    GOSUB MotorsSpinRight_180
  ENDIF
  RETURN


' -------------------------------

MotorsOff:
  HIGH LeftRev
  HIGH LeftFwd
  HIGH RightRev
  HIGH RightFwd
  RETURN


' -------------------------------

MotorsForward:
  GOSUB MotorsOff
  LOW LeftFwd
  LOW RightFwd
  RETURN


' -------------------------------

MotorsReverse:
  GOSUB MotorsOff
  LOW LeftRev
  LOW RightRev
  RETURN


' -------------------------------

MotorsSpinRight_090:
  GOSUB MotorsOff
  LOW LeftFwd
  LOW RightRev
  PAUSE 445
  GOSUB MotorsOff
  RETURN


' -------------------------------

MotorsSpinRight_180:
  GOSUB MotorsOff
  LOW LeftFwd
  LOW RightRev
  PAUSE 895
  GOSUB MotorsOff
  RETURN


' -------------------------------

MotorsSpinLeft_090:
  GOSUB MotorsOff
  LOW LeftRev
  LOW RightFwd
  PAUSE 445
  GOSUB MotorsOff
  RETURN


' -------------------------------

MotorsSpinLeft_180:
  GOSUB MotorsOff
  LOW LeftRev
  LOW RightFwd
  PAUSE 895
  GOSUB MotorsOff
  RETURN


' -------------------------------

#IF SIMMETRY_LCD = 1 #THEN
InitSimmetry:
  DEBUG "InitSimmetry:  baud", DEC baud, CR
  SEROUT SimmetryOut, Baud, ["W1", CR]
  PAUSE 10
  SEROUT SimmetryOut, Baud, ["C", CR]
  PAUSE 10
  SEROUT SimmetryOut, Baud, ["(", CR]
  PAUSE 10
  SEROUT SimmetryOut, Baud, ["I40", CR]
  PAUSE 10
  SEROUT SimmetryOut, Baud, ["Tm11,MazeRunner", CR]
  PAUSE 10
  SEROUT SimmetryOut, Baud, ["Tm31,Front ###cm X", CR]
  PAUSE 10
  SEROUT SimmetryOut, Baud, ["Tm51,Left  ###cm X", CR]
  PAUSE 10
  SEROUT SimmetryOut, Baud, ["Tm71,Right ###cm X", CR]
  PAUSE 10
  RETURN

UpdateSimmetry:
  SEROUT SimmetryOut, Baud, ["Nm37,", DEC3 cmIR, CR]
  PAUSE 5
  SEROUT SimmetryOut, Baud, ["Nm57,", DEC3 rawSonar2, CR]
  PAUSE 5
  SEROUT SimmetryOut, Baud, ["Nm77,", DEC3 rawSonar1, CR]
  PAUSE 5

  SEROUT SimmetryOut, Baud, ["Nm313,", DEC1 OpeningAhead, CR]
  PAUSE 5
  SEROUT SimmetryOut, Baud, ["Nm513,", DEC1 OpeningOnLeft, CR]
  PAUSE 5
  SEROUT SimmetryOut, Baud, ["Nm713,", DEC1 OpeningOnRight, CR]
  PAUSE 5

  RETURN
#ENDIF


' -------------------------------

#IF DEBUG_OUTPUT = 1 #THEN
InitDebug:
  DEBUG CLS,
        "MazeRunner", CR,
        "========================", CR,
        "Sensor    Dist(cm)  Open", CR,
        "Front", CR,
        "Left ", CR,
        "Right", CR,
        CR,
        "Action:"
  RETURN

UpdateDebug:
  DEBUG CRSRXY, 12, 3, DEC3 cmIR,      CLREOL, CRSRX, 22, DEC OpeningAhead,
        CRSRXY, 12, 4, DEC3 rawSonar2, CLREOL, CRSRX, 22, DEC OpeningOnLeft,
        CRSRXY, 12, 5, DEC3 rawSonar1, CLREOL, CRSRX, 22, DEC OpeningOnRight
  RETURN

ReportDebug_TurnRight:
  DEBUG CRSRXY, 10, 7, "Turning Right", CLREOL
  RETURN

ReportDebug_TurnLeft:
  DEBUG CRSRXY, 10, 7, "Turning Left", CLREOL
  RETURN

ReportDebug_TurnAround:
  DEBUG CRSRXY, 10, 7, "Turning Around", CLREOL
  RETURN

ReportDebug_MoveForward:
  DEBUG CRSRXY, 10, 7, "Moving Forward", CLREOL
  RETURN
#ENDIF

' -----[ SubSystem Test Subroutines ]--------------------------------------

MotorTest:
  GOSUB MotorsForward
  PAUSE 2000
  GOSUB MotorsOff
  PAUSE 2000
  GOSUB MotorsReverse
  PAUSE 2000
  GOSUB MotorsOff
  PAUSE 2000
  GOSUB MotorsSpinRight_090
  PAUSE 2000
  GOSUB MotorsSpinLeft_090
  PAUSE 2000
  GOSUB MotorsSpinRight_180
  PAUSE 2000
  GOSUB MotorsSpinLeft_180
  PAUSE 2000
  END


' -------------------------------

PingTest:
  ' setup report screen
  DEBUG CLS,
        "Parallax Ping Sonar    ", CR,
        "=======================", CR,
        "Centimeters......      ", CR,
        "OpeningOnLeft....      ", CR,
        "counter..........      ", CR
  counter = 0
  DO
    counter = counter + 1
    ' exercise the sensor
    GOSUB CheckLeftSensor
    ' update report screen
    DEBUG CRSRXY, 18, 2,
          DEC rawSonar2, CLREOL,
          CRSRXY, 18, 3,
          DEC OpeningOnLeft, CLREOL,
          CRSRXY, 18, 4,
          DEC3 counter, CLREOL
    PAUSE 500
  LOOP
  END

' -------------------------------

DevantechSRF04Test:
  ' setup report screen
  DEBUG CLS,
        "Devantech SRF04 Sonar  ", CR,
        "=======================", CR,
        "Centimeters......      ", CR,
        "OpeningOnRight...      ", CR,
        "counter..........      ", CR
  counter = 0
  DO
    counter = counter + 1
    ' exercise the sensor
    GOSUB CheckRightSensor
    ' update report screen
    DEBUG CRSRXY, 18, 2,
          DEC rawSonar1, CLREOL,
          CRSRXY, 18, 3,
          DEC OpeningOnRight, CLREOL,
          CRSRXY, 18, 4,
          DEC3 counter, CLREOL
    PAUSE 500
  LOOP
  END


' -------------------------------

SharpGP2D02Test:
  ' setup report screen
  DEBUG CLS,
        "Sharp GP2D02 Infrared Object Detector", CR,
        "=====================================", CR,
        "Centimeters......      ", CR,
        "OpeningAhead.....      ", CR,
        "counter..........      ", CR
  counter = 0
  DO
    counter = counter + 1
    ' exercise the sensor
    GOSUB CheckFrontSensor
    ' update report screen
    DEBUG CRSRXY, 18, 2,
          DEC cmIR, "  (", DEC inIR, " in.)", CLREOL,
          CRSRXY, 18, 3,
          BIN1 OpeningAhead, CLREOL,
          CRSRXY, 18, 4,
          DEC3 counter, CLREOL
    PAUSE 500
  LOOP
  END


'     --------------------------     '
'    /                          \    '
' --<   End of Program Listing   >-- '
'    \                          /    '
'     --------------------------     '