Combining Three Boe Bot Programs?
I have already grouped the variables and made sure no doubles are there. Im wondering how to combine these 3 programs into 1.
' Robotics with the Boe-Bot - FastIrRoaming.bs2
' Higher performance IR object detection assisted navigation
' {$STAMP BS2}
' {$PBASIC 2.5}
DEBUG "Program Running!"
irDetectLeft VAR Bit ' Variable Declarations
irDetectRight VAR Bit
pulseLeft VAR Word
pulseRight VAR Word
FREQOUT 4, 2000, 3000 ' Signal program start/reset.
DO ' Main Routine
FREQOUT 8, 1, 38500 ' Check IR Detectors
irDetectLeft = IN9
FREQOUT 2, 1, 38500
irDetectRight = IN0
' Decide how to navigate.
IF (irDetectLeft = 0) AND (irDetectRight = 0) THEN
pulseLeft = 650
pulseRight = 850
ELSEIF (irDetectLeft = 0) THEN
pulseLeft = 850
pulseRight = 850
ELSEIF (irDetectRight = 0) THEN
pulseLeft = 650
pulseRight = 650
ELSE
pulseLeft = 850
pulseRight = 650
ENDIF
PULSOUT 13,pulseLeft ' Apply the pulse.
PULSOUT 12,pulseRight
PAUSE 15
LOOP ' Repeat Main Routine
'
[ Title ]
' Robotics with the Boe-Bot - LightSeekingBoeBot.bs2
' Turns away from shadows and seeks out light sources. Able to detect shadows
' and navigate in most classroom ambient light levels. Although the system
' can adapt to a variety of ambient light levels, direct sunlight and rooms
' without any lights on will still exceed the system's ability to adjust and
' result in aimless roaming.
' {$STAMP BS2} ' Stamp directive.
' {$PBASIC 2.5} ' PBASIC directive.
'
[ Constants/Variables ]
NEGATIVE CON 1 ' For negative numbers
' Application Variables
pulseLeft VAR Word ' Left servo pulse duration
pulseRight VAR Word ' Right servo pulse duration
light VAR Word ' Brightness/darkness indicator
ndShade VAR Word ' Normalized differential shade
' Subroutine Variables
tLeft VAR pulseLeft ' Stores left RCTIME measurement
tRight VAR pulseRight ' Stores right RCTIME measurement
n VAR tLeft ' Numerator
d VAR Word ' Denominator
q VAR ndShade ' Quotient
sumDiff VAR d ' For sum and difference calcs
duty VAR Byte ' PWM duty argument variable
i VAR Nib ' Index counting variable
temp VAR i ' Temp storage for calcs
sign VAR Bit ' Var.BIT15 = 1 if neg, 0 if pos
'
[ Initialization ]
FREQOUT 4, 2000, 3000 ' Start beep
DEBUG "Program running..." ' Display program running message
'
[ Main Routine ]
DO ' Main Loop.
GOSUB Light_Shade_Info ' Get light & ndShade
' Navigation Routine
IF (ndShade + 500) > 500 THEN ' If more shade on right...
pulseLeft = 900 - ndShade MIN 650 MAX 850' Slow left wheel w/ right shade
pulseRight = 650 ' Right wheel full speed forward
ELSE ' If more shade on left...
pulseLeft = 850 ' Left wheel full speed forward
pulseRight= 600 - ndShade MIN 650 MAX 850' Slow right wheel w/ left shade
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)
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) / 200 ' 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
' {$STAMP BS2}
' {$PBASIC 2.5}
DEBUG "Program Running!"
'
[ Variables ]
pulseCount VAR Byte
counter VAR Nib
old7 VAR Bit
old5 VAR Bit
'
[ Initialization ]
FREQOUT 4, 2000, 3000
counter = 1
old7 = 0
old5 = 1
'
[ Main Routine ]
DO
' --- Detect Consecutive Alternate Corners
IF (IN7 <> IN5) THEN
IF (old7 <> IN7) AND (old5 <> IN5) THEN
counter = counter + 1
old7 = IN7
old5 = IN5
IF (counter > 4) THEN
counter = 1
GOSUB Back_Up
GOSUB Turn_Left
GOSUB Turn_Left
ENDIF
ELSE
counter = 1
ENDIF
ENDIF
IF (IN5 = 0) AND (IN7 = 0) THEN
GOSUB Back_Up
GOSUB Turn_Left
GOSUB Turn_Left
ELSEIF (IN5 = 0) THEN
GOSUB Back_Up
GOSUB Turn_Right
ELSEIF (IN7 = 0) THEN
GOSUB Back_Up
GOSUB Turn_Left
ELSE
GOSUB Forward_Pulse
ENDIF
LOOP
'
[ Subroutines ]
Forward_Pulse:
PULSOUT 13, 850
PULSOUT 12, 650
PAUSE 20
RETURN
Turn_Left:
FOR pulseCount = 0 TO 18
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 20
NEXT
RETURN
Turn_Right:
FOR pulseCount = 0 TO 18
PULSOUT 13, 850
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN
Back_Up:
FOR pulseCount = 0 TO 20
PULSOUT 13, 650
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN
' Robotics with the Boe-Bot - FastIrRoaming.bs2
' Higher performance IR object detection assisted navigation
' {$STAMP BS2}
' {$PBASIC 2.5}
DEBUG "Program Running!"
irDetectLeft VAR Bit ' Variable Declarations
irDetectRight VAR Bit
pulseLeft VAR Word
pulseRight VAR Word
FREQOUT 4, 2000, 3000 ' Signal program start/reset.
DO ' Main Routine
FREQOUT 8, 1, 38500 ' Check IR Detectors
irDetectLeft = IN9
FREQOUT 2, 1, 38500
irDetectRight = IN0
' Decide how to navigate.
IF (irDetectLeft = 0) AND (irDetectRight = 0) THEN
pulseLeft = 650
pulseRight = 850
ELSEIF (irDetectLeft = 0) THEN
pulseLeft = 850
pulseRight = 850
ELSEIF (irDetectRight = 0) THEN
pulseLeft = 650
pulseRight = 650
ELSE
pulseLeft = 850
pulseRight = 650
ENDIF
PULSOUT 13,pulseLeft ' Apply the pulse.
PULSOUT 12,pulseRight
PAUSE 15
LOOP ' Repeat Main Routine
'
[ Title ]
' Robotics with the Boe-Bot - LightSeekingBoeBot.bs2
' Turns away from shadows and seeks out light sources. Able to detect shadows
' and navigate in most classroom ambient light levels. Although the system
' can adapt to a variety of ambient light levels, direct sunlight and rooms
' without any lights on will still exceed the system's ability to adjust and
' result in aimless roaming.
' {$STAMP BS2} ' Stamp directive.
' {$PBASIC 2.5} ' PBASIC directive.
'
[ Constants/Variables ]
NEGATIVE CON 1 ' For negative numbers
' Application Variables
pulseLeft VAR Word ' Left servo pulse duration
pulseRight VAR Word ' Right servo pulse duration
light VAR Word ' Brightness/darkness indicator
ndShade VAR Word ' Normalized differential shade
' Subroutine Variables
tLeft VAR pulseLeft ' Stores left RCTIME measurement
tRight VAR pulseRight ' Stores right RCTIME measurement
n VAR tLeft ' Numerator
d VAR Word ' Denominator
q VAR ndShade ' Quotient
sumDiff VAR d ' For sum and difference calcs
duty VAR Byte ' PWM duty argument variable
i VAR Nib ' Index counting variable
temp VAR i ' Temp storage for calcs
sign VAR Bit ' Var.BIT15 = 1 if neg, 0 if pos
'
[ Initialization ]
FREQOUT 4, 2000, 3000 ' Start beep
DEBUG "Program running..." ' Display program running message
'
[ Main Routine ]
DO ' Main Loop.
GOSUB Light_Shade_Info ' Get light & ndShade
' Navigation Routine
IF (ndShade + 500) > 500 THEN ' If more shade on right...
pulseLeft = 900 - ndShade MIN 650 MAX 850' Slow left wheel w/ right shade
pulseRight = 650 ' Right wheel full speed forward
ELSE ' If more shade on left...
pulseLeft = 850 ' Left wheel full speed forward
pulseRight= 600 - ndShade MIN 650 MAX 850' Slow right wheel w/ left shade
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)
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) / 200 ' 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
' {$STAMP BS2}
' {$PBASIC 2.5}
DEBUG "Program Running!"
'
[ Variables ]
pulseCount VAR Byte
counter VAR Nib
old7 VAR Bit
old5 VAR Bit
'
[ Initialization ]
FREQOUT 4, 2000, 3000
counter = 1
old7 = 0
old5 = 1
'
[ Main Routine ]
DO
' --- Detect Consecutive Alternate Corners
IF (IN7 <> IN5) THEN
IF (old7 <> IN7) AND (old5 <> IN5) THEN
counter = counter + 1
old7 = IN7
old5 = IN5
IF (counter > 4) THEN
counter = 1
GOSUB Back_Up
GOSUB Turn_Left
GOSUB Turn_Left
ENDIF
ELSE
counter = 1
ENDIF
ENDIF
IF (IN5 = 0) AND (IN7 = 0) THEN
GOSUB Back_Up
GOSUB Turn_Left
GOSUB Turn_Left
ELSEIF (IN5 = 0) THEN
GOSUB Back_Up
GOSUB Turn_Right
ELSEIF (IN7 = 0) THEN
GOSUB Back_Up
GOSUB Turn_Left
ELSE
GOSUB Forward_Pulse
ENDIF
LOOP
'
[ Subroutines ]
Forward_Pulse:
PULSOUT 13, 850
PULSOUT 12, 650
PAUSE 20
RETURN
Turn_Left:
FOR pulseCount = 0 TO 18
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 20
NEXT
RETURN
Turn_Right:
FOR pulseCount = 0 TO 18
PULSOUT 13, 850
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN
Back_Up:
FOR pulseCount = 0 TO 20
PULSOUT 13, 650
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN
Comments
You need to restructure the program to check each sensor in the main loop and then perform the action based on sensor reading. Your program should check the Infrared and move away if necessary, then check the whiskers and move away if necessary, and finally using the phototransistor move in the direction of the strongest light.
I hope you understand that not all objects reflect infrared well and that sunlight and fluorescent lights often give off infrared that can confuse the sensor. Also the intensity of your light source affects the reaction point in your program.
Finally, servo pulses should be delivered about every 20 milliseconds so you will probably need to lower the pause time since you are doing more things each loop. You left off most of the program comments so it's more difficult to tell what piece of code does what. It's always good practice to comment your code and use meaningful variable names whenever possible.