'-----[ Title ]---------------------------------------------------------------
' Robotics with the Boe-Bot - LightSensorValues.bs2
' Displays conditioned ambient light level and differential shade on a scale
' of -500 to 500.

' {$STAMP BS2}                               ' Stamp directive.
' {$PBASIC 2.5}                              ' PBASIC directive.

'-----[ Constants/Variables ]-------------------------------------------------

Negative       CON     1                     ' For negative numbers

' Application Variables
light          VAR     Word                  ' Brightness/darkness indicator
ndShade        VAR     Word                  ' Normalized differential shade

' Subroutine Variables
tLeft          VAR     Word                  ' Stores left RCTIME measurement
tRight         VAR     Word                  ' Stores right RCTIME measurement
n              VAR     Word                  ' Numerator
d              VAR     Word                  ' Denominator
q              VAR     Word                  ' Quotient
sumDiff        VAR     Word                  ' For sum and difference calcs
duty           VAR     Byte                  ' PWM duty argument variable
i              VAR     Nib                   ' Index counting variable
temp           VAR     Nib                   ' Temp storage for calcs
sign           VAR     Bit                   ' Var.BIT15 = 1 if neg, 0 if pos

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

FREQOUT 4, 2000, 3000                        ' Start beep

DEBUG CLS                                    ' Clear Debug Terminal

'-----[ Main Routine ]--------------------------------------------------------

DO
                                             ' Main Loop.
  GOSUB Light_Shade_Info                     ' Get light & ndShade

  DEBUG HOME, "light = ", DEC3 light,        ' Display light & ndShade
    "/324", CLREOL, CR,
    "ndShade = ", SDEC3  ndShade, CLREOL

  PAUSE 100                                  ' Delay 0.1 seconds

LOOP                                         ' Repeat main loop

'-----[ Subroutine - Light_Shade_Info ]---------------------------------------
' Uses tLeft and tRight (RCTIME measurements) and pwm var to calculate:
'   o light   - Ambient light level on a scale of 0 to 324
'   o ndShade - Normalized differential shade on a scale of -500 to + 500
'               (-500 -> dark shade over left, 0 -> equal shade,
'                +500 -> dark shade over right)

Light_Shade_Info:                            ' Subroutine label
  GOSUB Light_Sensors                        ' Get raw RC light measurements
  sumdiff = (tLeft + tRight) MAX 65535       ' Start light level with sum
  IF duty <= 70 THEN                         ' If duty at min
    light=duty-(sumdiff/905) MIN 1           ' Find how much darker
    IF sumdiff = 0 THEN light = 0            ' If timeout, max darkness
  ELSEIF duty = 255 THEN                     ' If duty at max
    light=duty+((1800-(sumdiff))/26)         ' Find how much brighter
  ELSE                                       ' If duty in range
    light = duty                             ' light = duty
  ENDIF                                      ' Done with light level
  GOSUB Duty_Auto_Adjust                     ' Adjust PWM duty for next loop
  n = tLeft                                  ' Set up tLeft/(tLeft+tRight)
  d = tLeft + tRight
  GOSUB Fraction_Thousandths                 ' Divide (returns thousandths)
  ndShade = 500-q                            ' Normalized differential shade
  RETURN                                     ' Return from subroutine

'-----[ Subroutine - Light_Sensors ]------------------------------------------
' Measure P6 and P3 light sensor circuits.  Duty variable must be in 70...255.
' Stores results in tLeft and tRight.

Light_Sensors:                               ' Subroutine label
  PWM 6, duty, 1                             ' Charge cap in P6 circuit
  RCTIME 6, 1, tLeft                         ' Measure P6 decay
  PWM 3, duty, 1                             ' Charge cap in P3 circuit
  RCTIME 3, 1, tRight                        ' Measure decay
  RETURN                                     ' Return from subroutine

'-----[ Subroutine - Duty_Auto_Adjust ]---------------------------------------
' Adjust duty variable to keep tLeft + tRight in the 1800 to 2200 range.
' Requires sumdiff word variable for calculations.

Duty_Auto_Adjust:                            ' Subroutine label
  sumDiff = (tLeft + tRight) MAX 4000        ' Limit max ambient value
  IF sumDiff = 0 THEN sumDiff = 4000         ' If 0 (timeout) then 4000
  IF (sumDiff<=1800) OR (sumDiff>=2200) THEN ' If outside 1800 to 2200
    sumDiff = 2000 - sumDiff                 ' Find excursion from target val
    sign = sumDiff.BIT15                     ' Pos/neg if .BIT15 = 0/1
    sumDiff = ABS(sumDiff) / 10              ' Max sumDiff will be +/- 10
    sumDiff = sumDiff MAX ((duty-68)/2)      ' Reduce adjustment increments
    sumDiff = sumDiff MAX ((257-duty)/2)     '  near ends of the range
    IF sign=NEGATIVE THEN sumDiff=-sumDiff   ' Restore sign
    duty = duty + sumDiff MIN 70 MAX 255     ' Limit duty to 70 to 255
  ENDIF                                      ' End of if outside 1800 to 2200
  RETURN                                     ' Return from subroutine