' Scribbler surface detection and behavior project
'   For Servo Magazine article
'   Copyright 2005, Element Products, Inc.
'
'  {$STAMP BS2}
'  {$PBASIC 2.5}




' I/O declarations

LightRight    PIN 0
LightCenter   PIN 1
LightLeft     PIN 2
LineEnable    PIN 3
LineRight     PIN 4
LineLeft      PIN 5
LedRight      PIN 8
LedCenter     PIN 9
LedLeft       PIN 10
Speaker       PIN 11
MotorRight    PIN 12
MotorLeft     PIN 13



' Constants

DriveVel CON 168
TurnVelFwd CON 173
TurnVelRev CON 83



' Behavior Priorities

BlackTurnP CON 6
SwitchP CON 5
GrayTurnP CON 4
WhiteDriveP CON 3
WhiteTurnP CON 2
GrayDriveP CON 1



' I/O initialization

LOW MotorRight
LOW MotorLeft
LOW LedRight
LOW LedCenter
LOW LedLeft



' Subroutine variables

light_level VAR Word
light_temp VAR Word
motor_r VAR Byte
motor_l VAR Byte
motor_temp VAR Byte



' Main program variables

light_start VAR Word
floor VAR Byte
index VAR Byte
light_on VAR Bit
last_light VAR Bit
behavior_p VAR Byte
switching VAR Bit
off_cnt VAR Byte



' Save space for calibration values

DATA@0, (16)




' Main program

start:
  HIGH LineEnable
  FREQOUT Speaker, 200, 500
  GOSUB checkLightLevel
  light_start = ((light_level * 5) / 2)
  behavior_p = 0
  motor_r = 128
  motor_l = 128
  last_light = 1
  PAUSE 3000

' Behavior loop

DO
  GOSUB checkSensors
  GOSUB blackTurnBeh
  GOSUB switchBeh
  GOSUB grayTurnBeh
  GOSUB whiteDriveBeh
  GOSUB whiteTurnBeh
  GOSUB grayDriveBeh
  GOSUB arbiter
LOOP




' Subroutines

checkSensors:
  GOSUB checkLightLevel
  IF(light_level < light_start) THEN light_on = 1 ELSE light_on = 0

  floor = 0
  FOR index = 0 TO 49
    IF(LineRight = 1) THEN floor = floor + 1
    IF(LineLeft = 1) THEN floor = floor + 1
  NEXT
  RETURN




blackTurnBeh:
  IF(floor > 99) THEN 
    IF(behavior_p < BlackTurnP) THEN 
      behavior_p = BlackTurnP
      motor_r = TurnVelRev
      motor_l = TurnVelFwd
    ENDIF
  ENDIF
  RETURN




switchBeh:
  IF(NOT (last_light = light_on)) THEN 
    switching = 1
    off_cnt = 0
  ENDIF
  last_light = light_on
  IF(switching) THEN FREQOUT Speaker, 50, 800

  IF(switching) THEN
    IF(behavior_p < SwitchP) THEN
      behavior_p = SwitchP
      motor_r = DriveVel
      motor_l = DriveVel
    ENDIF
    IF(((light_on = 1) AND (floor = 0)) OR ((light_on = 0) AND (floor > 0))) THEN
      off_cnt = off_cnt + 1
      IF(off_cnt > 8) THEN switching = 0
    ELSE
      off_cnt = 0
    ENDIF
  ENDIF
  RETURN




grayTurnBeh:
  IF((light_on = 1) AND (floor > 0)) THEN
    IF(behavior_p < GrayTurnP) THEN 
      behavior_p = GrayTurnP
      motor_r = TurnVelRev
      motor_l = TurnVelFwd
    ENDIF
  ENDIF
  RETURN




whiteDriveBeh:
  IF(light_on = 1) THEN
    IF(behavior_p < WhiteDriveP) THEN
      behavior_p = WhiteDriveP
      motor_r = DriveVel
      motor_l = DriveVel
    ENDIF
  ENDIF
  RETURN




whiteTurnBeh:
  IF(floor = 0) THEN
    IF(behavior_p < WhiteTurnP) THEN 
      behavior_p = WhiteTurnP
      motor_r = TurnVelRev
      motor_l = TurnVelFwd
    ENDIF
  ENDIF
  RETURN




grayDriveBeh:
  IF(behavior_p < GrayDriveP) THEN
    behavior_p = GrayDriveP
    motor_r = DriveVel
    motor_l = DriveVel
  ENDIF
  RETURN




arbiter:
  OUTC = (behavior_p & %0111)
  GOSUB motorSet
  behavior_p = 0
  RETURN




' Light level check subroutine
'   places the center light sensor value in light_level
'   lower values mean more light

checkLightLevel:
  HIGH LightRight
  HIGH LightCenter
  HIGH LightLeft
  PAUSE 3

  light_level = 0
  RCTIME LightRight, 1, light_temp
  light_level = light_level + light_temp
  RCTIME LightCenter, 1, light_temp
  light_level = light_level + light_temp
  RCTIME LightLeft, 1, light_temp
  light_level = light_level + light_temp
  RETURN




' Motor command subroutine
'
' takes two parameters:
'   motor_r = speed of the right motor (28 - 228)
'     (28 = full reverse, 128 = stop, 228 = full forward)
'   motor_l = speed of the left motor (28 - 228)
'     (28 = full reverse, 128 = stop, 228 = full forward)
'
' corrects the motor speed according to the correction factors
'   stored in address 0 (right) and 1 (left) of the EEPROM.
'   The correction factors are in the range 0 - 255.
'    64 = correction factor = 0.5
'   128 = correction factor = 1.0 (normal speed)
'   192 = correction factor = 1.5

motorSet:
  READ 0, motor_temp
  PULSOUT MotorRight, (((((((motor_r - 128) * motor_temp) >> 7) + 256) & 511) MIN 156) * 10) - 560
  PAUSE 1
  READ 1, motor_temp
  PULSOUT MotorLeft, (((((((motor_l - 128) * motor_temp) >> 7) + 256) & 511) MIN 156) * 10) - 560
  RETURN