' {$STAMP BS2sx}
' {$PBASIC 2.5}

' BIT                 VAR         0-1
' NIB                 VAR         0-15
' BYTE                VAR         0-255
' WORD                VAR         0-65535

' Scan values on Horzontal plane are as follows
' Far Right Max            = 700
' Right 45 degrees         = 1100
' Middle                   = 1500
' Left 45 degrees          = 1900
' Far Left Max             = 2300


baud                  CON         240                            ' baud rate for servo controller board
SerialCom             CON         5                              ' Pin for comunications to servo controller board
Alarm                 CON         14
SAlarm                CON         7                              ' Alarm pin
Apos                  CON         8
Bpos                  CON         9
Aplus                 CON         10
Amin                  CON         11
BPlus                 CON         12
Bmin                  CON         13
AdcDo                 PIN         2
AdcCLK                PIN         1
AdcCS                 PIN         0
VRef                  PIN         3

RawData               VAR         Word
AvgData               VAR         Word
PANVALUE              VAR         Word

ScrapX                VAR         Byte
DIRECTION             VAR         Nib
DIGIT                 VAR         Byte
Memo                  VAR         Byte(5)
x                     VAR         Nib
Y                     VAR         Nib
ThreatCond            VAR         Byte(5)



Main:
  PANVALUE = 1500
  GOSUB MUTE
  GOSUB OFFSET                                                   ' SET SERVO OFFSET
  GOSUB SETSERVO                                                 ' SET SERVOS JUST ON STARTUP
  GOSUB PAN
  GOSUB DISTANCE
  GOSUB TESTIT
  GOSUB FINDDIRECTION
  GOSUB LOGIC
  GOSUB DISPLAY
END

MUTE:
  PAUSE 3000
  RETURN
OFFSET:                                                          ' Sets the offset on the SSC-32 servo controller
  SEROUT SerialCom, baud, ["#30Po","200", CR]                    ' Horazontial plane
  PAUSE 200
  RETURN
SETSERVO:
  SEROUT SerialCom, baud, ["#30","P1500","T10", CR]              ' Horazontial plane
  SEROUT SerialCom, baud, ["#31","P1600","T10", CR]              ' Horazontial plane
  RETURN
Pan:
  SEROUT SerialCom, baud, ["#30P",DEC PANVALUE,"T1000", CR]      ' Horazontial plane
  PAUSE 1000
  RETURN
Distance:
    FOR ScrapX = 0 TO 5
    HIGH Vref                                                    '  Vref set to max Voltage to measure close distances
    LOW AdcCs
    SHIFTIN AdcDo, AdcClk, MSBPOST, [RawData \9]
    HIGH AdcCs
    PAUSE 1
    AvgData = AvgData + RawData
    NEXT
    AvgData = AvgData / 6
    PAUSE 20
    RETURN
TestIt:
  PANVALUE = 700                                                 'far right
  GOSUB pan
  GOSUB distance
  Memo(0) = AvgData
  PAUSE 1000
  DEBUG HOME, "LEFT  90 = ", SDEC memo(0)

  PANVALUE = 1100                                                ' right 45 degrees
  GOSUB pan
  GOSUB distance
  Memo(1) = AvgData
  PAUSE 1000
  DEBUG CR, "LEFT  45 = ", SDEC memo(1)

  PANVALUE = 1900                                                 'far left 45 degrees
  GOSUB pan
  GOSUB distance
  Memo(3) = AvgData
  PAUSE 1000
  DEBUG CR, "RIGHT 45 = ", SDEC memo(3)

  PANVALUE = 2300                                                 'far left
  GOSUB pan
  GOSUB distance
  Memo(4) = AvgData
  PAUSE 1000
  DEBUG CR, "RIGHT 90 = ", SDEC memo(4)

  PANVALUE = 1500                                                  'Middle
  GOSUB pan
  GOSUB distance
  Memo(2) = AvgData
  PAUSE 1000
  DEBUG CR, "MIDDLE 0= ", SDEC memo(2)

  FindDirection:
                                                                   ' 250  THREAT = 5
                                                                   ' 200  THREAT = 4
                                                                   ' 150  THREAT = 3
                                                                   ' 100  THREAT = 2
                                                                   ' 50   THREAT = 1
                                                                   ' MEMO(0) = LEFT   90 DEGREES
                                                                   ' MEMO(1) = LEFT   45 DEGREES
                                                                   ' MEMO(2) = MIDDLE  0 DEGREES
                                                                   ' MEMO(3) = RIGHT  45 DEGREES
                                                                   ' MEMO(4) = RIGHT  90 DEGREES

  X = 0
  DIGIT = 250
  DO UNTIL (X > 4)
  IF memo(0) < (DIGIT) THEN THREATCOND(0) = (5 - x)
  DIGIT = DIGIT - 50
  X = X + 1
  LOOP

  X = 0
  DIGIT = 250
  DO UNTIL (X > 4)
  IF memo(1) < (DIGIT) THEN THREATCOND(1) = (5 - x)
  DIGIT = DIGIT - 50
  X = X + 1
  LOOP


  X = 0
  DIGIT = 250
  DO UNTIL (X > 4)
  IF memo(2) < (DIGIT) THEN THREATCOND(2) = (5 - x)
  DIGIT = DIGIT - 50
  X = X + 1
  LOOP

  X = 0
  DIGIT = 250
  DO UNTIL (X > 4)
  IF memo(3) < (DIGIT) THEN THREATCOND(3) = (5 - x)
  DIGIT = DIGIT - 50
  X = X + 1
  LOOP

  X = 0
  DIGIT = 250
  DO UNTIL (X > 4)
  IF memo(4) < (DIGIT) THEN THREATCOND(4) = (5 - x)
  DIGIT = DIGIT - 50
  X = X + 1
  LOOP
LOGIC:

  X = 0
  DIGIT = 3
  DO
  IF THREATCOND(X) <= DIGIT THEN DIRECTION = THREATCOND(X)
  X = X + 1
  LOOP UNTIL (X = 5)
  RETURN

DISPLAY:

  DEBUG CR, "THREAT CONDITION LEFT  90 DEGREES = ", DEC THREATCOND(0)
  DEBUG CR, "THREAT CONDITION LEFT  45 DEGREES = ", DEC THREATCOND(1)
  DEBUG CR, "THREAT CONDITION MIDD  00 DEGREES = ", DEC THREATCOND(2)
  DEBUG CR, "THREAT CONDITION RIGHT 45 DEGREES = ", DEC THREATCOND(3)
  DEBUG CR, "THREAT CONDITION RIGHT 90 DEGREES = ", DEC THREATCOND(4)
  DEBUG CR, " cHOOSE TO GO - " ,DEC DIRECTION
  END 