' {$STAMP BS2p}
' {$PBASIC 2.5}

'******************************************************************************
'Joshua Endicott
'Mechanical Engineering Technology Student
'WVU Institute of Technology
'******************************************************************************
' A HMC6352 compass module and sonar PING are used to steer a straight path
' and follow the a Wall
'
'.............................................................................
' Circuit Diagram
'..............................................................................

'  -------------
'     BS2      |                     +5v
'              |                      |
'              |            ----------------------
'              |           |                      |
'         pin 0|-----------| I/O                  |
'              |           |       Sonar          |
'              |           |                      |
'              |            ----------------------
'              |                    |
'              |                   ---
'              |                    -
'              |
'              |                     +5v
'              |                      |
'              |            ----------------------
'              |           |                      |
'         pin 9|-----------| Data                 |
'              |           |       HMC6352        |
'              |           |                      |
'         pin 8|-----------| Clock                |
'              |           |                      |
'              |            ----------------------
'              |                    |
'              |                   ---
'              |                    -
'              |
'        pin 13|-------------------------------------------
'              |                                           |
'        pin 12|------------------                     ____|____
'              |                 |              +5vdc |         |
'              |             ____|____             |  | Right   |
'              |      +5vdc |         |            ---| Servo   |
'              |         |  | Left    |               |         |
'              |         ---| Servo   |               |_________|
'              |            |         |                    |
'              |            |_________|                   ---
'              |                 |                         -
'              |                ---
'              |                 -
'______________|
'
'..............................................................................

'******************************************************************************
'  System Constants
'******************************************************************************
wallOffset        CON      2000
someTolerance     CON      100

' System Constants
'******************************************************************************
' Used by the Compass module
  HMC6352_READ   CON      $43
  HMC6352_WRITE  CON      $42
  GET_HEADING    CON      $41
  sck            CON      1
  sio            CON      0

'...Program Constants
  True           CON      1
  False          CON      0

  '-------------------------------------
  ' Number Ranges
  '.....................................
  '  range  0 -->   32766    positive
  '         +0  --> + 32766
  '.....................................
  '  range  65535 --> 32767  negative
  '         -1    --> -32767
  '.....................................
  MaxNum         CON      65535
  halfMaxNum     CON      32767

  '...Motor Constants
  '
  RightZeroSpeed CON      750
  LeftZeroSpeed  CON      750

  speed          CON      100
  turnspeed      CON      50

  '...Allowable drift      degrees *10
  delta          CON      50

'******************************************************************************
' I/O Pin Assignments
'******************************************************************************
  PING           PIN      0     ' Sonar

  SDA            PIN      9     ' Compass Data pin
  SCL            PIN      8     ' COmpas clock pin

  RightServo     PIN      13
  LeftServo      PIN      12    '

'******************************************************************************
'  Variables
'******************************************************************************
  Theta          VAR      Word ' Commanded heading
  Heading        VAR      Word ' Actual Heading
  h1             VAR      Word ' Past Headings for
  h2             VAR      Word ' data smoothing
  h3             VAR      Word
  h4             VAR      Word

  diff           VAR      Word ' Heading - Theta
  absDiff        VAR      Word ' ABS(Heading - Theta)
  dir            VAR      Bit  ' Left/Right direction flag

  ruleFired      VAR      Word  ' Rule Flag

  I2C_DATA       VAR      Byte ' Compass
  I2C_LSB        VAR      Bit  ' Compass

  sonarDistance  VAR      Word
  oldSonar       VAR      Word

'******************************************************************************
'  Initialization
'******************************************************************************
  PAUSE 200
  GOSUB Compass_Get_Heading    ' return Heading
  PAUSE 2000
  GOSUB Compass_Get_Heading    ' return Heading
  Theta = Heading              ' Target Heading

'******************************************************************************
'  Main Control Loop
'******************************************************************************
DO
   '--------------------------------------------
   ' get Sensors
   '--------------------------------------------
    GOSUB Compass_Get_Heading    'return Heading
    GOSUB getSonar

  '--------------------------------------------
   ' Condition Sensors   (Average 4 readings)
   '--------------------------------------------
    h1 = Heading
    Heading = (h1 + h2 +h3 + h4) /4  ' Average
    h4 = h3 : h3 = h2 : h2 = h1      ' Shift
    diff = Heading - Theta
    absDiff = ABS(diff)
    IF absDiff > 1800 THEN absDiff = 3600 - absdiff
    IF (absDiff + Theta)//3600 = Heading THEN :dir = 0 :ELSE :dir =1 :ENDIF

Rule1:
LOOP
'******************************************************************************
Compass_Get_Heading:  ' Compass module subroutine         return Heading
'------------------------------------------------------------------------------
  GOSUB I2C_Start
  I2C_DATA = HMC6352_WRITE
  GOSUB I2C_Write
  I2C_DATA = GET_HEADING
  GOSUB I2C_Write
  GOSUB I2C_Stop

  GOSUB I2C_Start
  I2C_DATA = HMC6352_READ
  GOSUB I2C_Write
  PAUSE 6
  GOSUB I2C_Read
  Heading.HIGHBYTE = I2C_DATA
  GOSUB I2C_ACK
  GOSUB I2C_Read
  Heading.LOWBYTE = I2C_DATA
  GOSUB I2C_NACK
  GOSUB I2C_Stop
RETURN
'..............................................................................

'------------------------------------------------------------------------------
I2C_Start:  ' Compass
'------------------------------------------------------------------------------
  LOW SDA
  LOW SCL
RETURN
'..............................................................................

'------------------------------------------------------------------------------
I2C_Stop:  ' Compass
'------------------------------------------------------------------------------
  LOW   SDA
  INPUT SCL
  INPUT SDA
RETURN
'..............................................................................

'------------------------------------------------------------------------------
I2C_ACK:  ' Compass
'------------------------------------------------------------------------------
  LOW   SDA
  INPUT SCL
  LOW   SCL
  INPUT SDA
RETURN
'..............................................................................

'------------------------------------------------------------------------------
I2C_NACK:  ' Compass
'------------------------------------------------------------------------------
  INPUT SDA
  INPUT SCL
  LOW   SCL
RETURN
'..............................................................................

'------------------------------------------------------------------------------
I2C_Read:  ' Compass
'------------------------------------------------------------------------------
  SHIFTIN SDA, SCL, MSBPRE, [I2C_DATA]
RETURN
'..............................................................................

'------------------------------------------------------------------------------
I2C_Write:  ' Compass
'------------------------------------------------------------------------------
  I2C_LSB = I2C_DATA.BIT0
  I2C_DATA = I2C_DATA / 2
  SHIFTOUT SDA, SCL, MSBFIRST, [I2C_DATA\7]
  IF I2C_LSB THEN INPUT SDA ELSE LOW SDA
  INPUT SCL
  LOW SCL
  INPUT SDA
  INPUT SCL
  LOW SCL
RETURN
'..............................................................................

'******************************************************************************
'  getSonar       return sonarDistnce
'******************************************************************************
getSonar:
  PING = 0                         ' make trigger 0-1-0
  PULSOUT PING, 13                 ' activate sensor
  PULSIN  PING, 1, sonarDistance   ' measure echo pulse

  '... Make sure a non zero reading is obtained
  IF sonarDistance = 0 THEN sonarDistance = oldSonar
  oldSonar = sonarDistance

RETURN
'..............................................................................