'-----[ Title ]---------------------------------------------------------------
' Robotics with the Boe-Bot - EscapingLight.bs2

' {$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
counter VAR Word
' Application Variables
 pulseLeft VAR Word
 pulseRight VAR Word

'-----[ Initialization ]------------------------------------------------------
PAUSE 5000

DEBUG "Program running..." ' Display program running message
'-----[ Main Routine ]--------------------------------------------------------
DO ' Main Loop.
 GOSUB Light_Shade_Info ' Get light & ndShade
 ndShade = -ndShade

' Navigation Routine
 IF (ndShade + 500) > 500 THEN ' If more shade on left...
pulseLeft = 750
pulseRight = 650
 ELSE ' If more shade on right...
 pulseLeft = 850 ' Left wheel full speed forward
 pulseRight= 750
 ENDIF
 PULSOUT 13, pulseLeft ' Left servo control pulse
 PULSOUT 12, pulseRight ' Right servo control pulse
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)





Pan_Left:
      PULSOUT 13, 750                            ' Pulse servos in circular path
      PULSOUT 12, 650



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

 '-----[ Subroutine - Fraction_Thousandths ]-----------------------------------
' Calculate q = n/d as a number of thousandths.
' n and d should be unsigned and n < d. Requires Nib size temp & i variables.
Fraction_Thousandths: ' Subroutine label
 q = 0 ' Clear quotient
 IF n > 6500 THEN ' If n > 6500
 temp = n / 6500 ' scale n into 0..6500
 n = n / temp
 d = d / temp ' scale d with n
 ENDIF
 FOR i = 0 TO 3 ' Long division ten thousandths
 n = n // d * 10 ' Multiply remainder by 10
 q = q * 10 + (n/d) ' Add next digit to quotient
 NEXT
 IF q//10>=5 THEN q=q/10+1 ELSE q=q/10 ' Round q to nearest thousandth
 RETURN ' Return from subroutine