Corner Escape Logic using IR Sensors and
SteveWoodrough
Posts: 190
I'm a newbie.· I've plowed just about all the way through Robotics with the BOE-BOT, and I have a pretty basic understanding of the device.· I'm familiar with the corner escape·logic using whiskers and I'm attempting to adapt that logic with the IR sensors.
I have succesfully adapted the sample whisker sample program to work with the IR sensors. Now I need help with corner escape logic.
I have a Bot with 2 IR sensors that navigates in my 4'X 5' "arena" just fine.· It·"bounces" off the walls and wanders wonderfully.· I'm using CASE logic and the "fast nav" approach where by I send a PULSOUT command with each LOOP.
··
Using whiskers, recall that a FOR NEXT loop was typically used.·In a corner situation the FOR NEXT loop tended to drive the bot from right to left and back right again etc. ·with little chance of NOT bouncing back and forth and engaging the "Corner Escape" logic.
In the scenerio I have now (pulsout with each loop) the bot can "fibrulate"·in a corner situation.· On RARE occasion I'll get lucky and the bot will go left, right, left, right each time in it's own LOOP and activate the corner escape logic.· Many times one sensor remains on for succesive loops or both sensors go off for 1 or more LOOP cycles.·The result is that the corner escape logic resets and even though the logic conditions of being trapped in a corner are not meet the BOT is Trapped!··How do I account for this condition? In my head I want the logic to be that any succession of alternating left right commands in some period (say 250 msec) would result in a corner escape command.· So for example: If over a 250 msec period the move command is left right left right left right then GOSUB U_Turn.· If over a 250msec period the move command is left, left, right, left, right, right, left, right again GUSUB U_Turn.·
My logic would be along the lines of: IF there are 3 lefts AND 3 rights in some time period (1/4 second) then assume corner trapped and execute the escape routine (U_Turn)
How do I write that?·
Will the COUNT command work? HOW?
Does someone have a code sample they can share?
Thank You for your time and your help.
Regards...Steve Woodrough
'
[noparse][[/noparse] Title ]
'{$STAMP BS2}
'{$PBASIC 2.5}
' IR Sensor Navigator
' CASE logic
' Indicator LED's
' Corner Escape logic
'
[noparse][[/noparse] Variables ]
qti VAR Nib
PulseLeft VAR Word
PulseRight VAR Word
irDetectLeft VAR Bit
irDetectRight VAR Bit
pulseCount VAR Byte ' FOR...NEXT loop counter.
counter VAR Nib ' Counts alternate contacts.
old9 VAR Bit ' Stores previous IN9 Left IR Sensor output
old0 VAR Bit ' Stores previous IN0 Right Sensor output
'
[noparse][[/noparse] Initialization ]
FREQOUT 4, 200, 3000 ' Signal program start/reset.
counter = 1 ' Start alternate corner count.
old9 = 1 ' Make up old values.· Must be different from each other
old0 = 0
'
[noparse][[/noparse] Main Routine ]
DO
Right: FREQOUT 2, 1, 38500:irDetectRight=IN0:qti.BIT0=irDetectRight: INPUT 0 ' These INPUT commands might be optional
Left: FREQOUT 8, 1, 38500 : irDetectLeft=IN9:qti.BIT1=irDetectLeft:INPUT 9
Wall: FREQOUT 14, 1, 38500: qti.BIT2=IN15: INPUT 15
' --- Detect Consecutive Alternate Corners
' See the "How EscapingCorners.bs2 Works" section that follows this program.
IF (irDetectLeft <> irDetectRight) THEN ' One or other is pressed.
IF (old9 <> irDetectLeft) AND (old0 <> irDetectRight) THEN ' Different from previous.
counter = counter + 1 ' Alternate whisker count + 1.
old9 = irDetectLeft ' Record this whisker press
old0 = irDetectRight ' for next comparison.
IF (counter > 3) THEN ' If alternate whisker count = 3,
counter = 1 ' reset whisker counter
GOSUB U_Turn ' and execute a U-turn.
ENDIF ' ENDIF counter > 3.
ELSE ' ELSE (old9=irDetectLeft) or (old0=irDetectRight),
counter = 1 ' not alternate, reset counter.
ENDIF ' ENDIF (old9<>irDetectLeft) and
' (old0<>irDetectRight).
ENDIF ' ENDIF (irDetectLeft<>irDetectRight).
SELECT qti
'++++++++++++· 3 LED Scenario +++++++· Don't forget 0 means blocked!!!
CASE %000· 'All sensors closed· Test OK
HIGH 11:HIGH 10:HIGH 1
GOSUB BACK_UP
CASE %001·· 'WALL and LEFT detect RIGHT open Test OK
HIGH 11:HIGH 10:LOW 1
GOSUB Turn_right
CASE %010 'WALL Detect LEFT open RIGHT detect Test OK
HIGH 11:LOW 10:HIGH 1
GOSUB Turn_right
CASE %011· 'WALL detect LEFT and RIGHT open Test OK
HIGH 11:LOW 10:LOW 1
GOSUB Go_Straight
CASE %100 'WALL open LEFT and RIGHT Detect Test OK
LOW 11:HIGH 10:HIGH 1
GOSUB Turn_right
CASE %101· 'WALL open LEFT detect RIGHT open· Test OK
LOW 11:HIGH 10:LOW 1
GOSUB Turn_right
CASE %110· 'WALL and LEFT open RIGHT detect Test OK
LOW 11:LOW 10:HIGH 1
GOSUB Turn_Left
CASE %111 'Wall open LEFT open· RIGHT open Test OK
LOW 11:LOW 10:LOW 1
GOSUB Go_Straight
ENDSELECT
PAUSE 1000
PULSOUT 13,pulseLeft ' Apply the pulse.
PULSOUT 12,pulseRight
LOOP
'++++++Movement subroutines
TURN_RIGHT:
pulseLeft = 850········ 'TURN RIGHT
pulseRight = 850
RETURN
TURN_LEFT:
pulseLeft = 650········ 'TURN LEFT
pulseRight = 650
RETURN
GO_STRAIGHT:
pulseLeft = 850··· 'Go staight
pulseRight = 650
RETURN
BACK_UP:
pulseLeft = 650····· 'BACK UP
pulseRight = 850
RETURN
U_Turn:
·FREQOUT 4, 2000, 6000
·· ' Back up.
FOR pulseCount = 0 TO 40
PULSOUT 13, 650
PULSOUT 12, 850
PAUSE 20
NEXT
FOR pulseCount = 0 TO 20 ' Right turn, about 90-degrees.
PULSOUT 13, 850
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN
I have succesfully adapted the sample whisker sample program to work with the IR sensors. Now I need help with corner escape logic.
I have a Bot with 2 IR sensors that navigates in my 4'X 5' "arena" just fine.· It·"bounces" off the walls and wanders wonderfully.· I'm using CASE logic and the "fast nav" approach where by I send a PULSOUT command with each LOOP.
··
Using whiskers, recall that a FOR NEXT loop was typically used.·In a corner situation the FOR NEXT loop tended to drive the bot from right to left and back right again etc. ·with little chance of NOT bouncing back and forth and engaging the "Corner Escape" logic.
In the scenerio I have now (pulsout with each loop) the bot can "fibrulate"·in a corner situation.· On RARE occasion I'll get lucky and the bot will go left, right, left, right each time in it's own LOOP and activate the corner escape logic.· Many times one sensor remains on for succesive loops or both sensors go off for 1 or more LOOP cycles.·The result is that the corner escape logic resets and even though the logic conditions of being trapped in a corner are not meet the BOT is Trapped!··How do I account for this condition? In my head I want the logic to be that any succession of alternating left right commands in some period (say 250 msec) would result in a corner escape command.· So for example: If over a 250 msec period the move command is left right left right left right then GOSUB U_Turn.· If over a 250msec period the move command is left, left, right, left, right, right, left, right again GUSUB U_Turn.·
My logic would be along the lines of: IF there are 3 lefts AND 3 rights in some time period (1/4 second) then assume corner trapped and execute the escape routine (U_Turn)
How do I write that?·
Will the COUNT command work? HOW?
Does someone have a code sample they can share?
Thank You for your time and your help.
Regards...Steve Woodrough
'
[noparse][[/noparse] Title ]
'{$STAMP BS2}
'{$PBASIC 2.5}
' IR Sensor Navigator
' CASE logic
' Indicator LED's
' Corner Escape logic
'
[noparse][[/noparse] Variables ]
qti VAR Nib
PulseLeft VAR Word
PulseRight VAR Word
irDetectLeft VAR Bit
irDetectRight VAR Bit
pulseCount VAR Byte ' FOR...NEXT loop counter.
counter VAR Nib ' Counts alternate contacts.
old9 VAR Bit ' Stores previous IN9 Left IR Sensor output
old0 VAR Bit ' Stores previous IN0 Right Sensor output
'
[noparse][[/noparse] Initialization ]
FREQOUT 4, 200, 3000 ' Signal program start/reset.
counter = 1 ' Start alternate corner count.
old9 = 1 ' Make up old values.· Must be different from each other
old0 = 0
'
[noparse][[/noparse] Main Routine ]
DO
Right: FREQOUT 2, 1, 38500:irDetectRight=IN0:qti.BIT0=irDetectRight: INPUT 0 ' These INPUT commands might be optional
Left: FREQOUT 8, 1, 38500 : irDetectLeft=IN9:qti.BIT1=irDetectLeft:INPUT 9
Wall: FREQOUT 14, 1, 38500: qti.BIT2=IN15: INPUT 15
' --- Detect Consecutive Alternate Corners
' See the "How EscapingCorners.bs2 Works" section that follows this program.
IF (irDetectLeft <> irDetectRight) THEN ' One or other is pressed.
IF (old9 <> irDetectLeft) AND (old0 <> irDetectRight) THEN ' Different from previous.
counter = counter + 1 ' Alternate whisker count + 1.
old9 = irDetectLeft ' Record this whisker press
old0 = irDetectRight ' for next comparison.
IF (counter > 3) THEN ' If alternate whisker count = 3,
counter = 1 ' reset whisker counter
GOSUB U_Turn ' and execute a U-turn.
ENDIF ' ENDIF counter > 3.
ELSE ' ELSE (old9=irDetectLeft) or (old0=irDetectRight),
counter = 1 ' not alternate, reset counter.
ENDIF ' ENDIF (old9<>irDetectLeft) and
' (old0<>irDetectRight).
ENDIF ' ENDIF (irDetectLeft<>irDetectRight).
SELECT qti
'++++++++++++· 3 LED Scenario +++++++· Don't forget 0 means blocked!!!
CASE %000· 'All sensors closed· Test OK
HIGH 11:HIGH 10:HIGH 1
GOSUB BACK_UP
CASE %001·· 'WALL and LEFT detect RIGHT open Test OK
HIGH 11:HIGH 10:LOW 1
GOSUB Turn_right
CASE %010 'WALL Detect LEFT open RIGHT detect Test OK
HIGH 11:LOW 10:HIGH 1
GOSUB Turn_right
CASE %011· 'WALL detect LEFT and RIGHT open Test OK
HIGH 11:LOW 10:LOW 1
GOSUB Go_Straight
CASE %100 'WALL open LEFT and RIGHT Detect Test OK
LOW 11:HIGH 10:HIGH 1
GOSUB Turn_right
CASE %101· 'WALL open LEFT detect RIGHT open· Test OK
LOW 11:HIGH 10:LOW 1
GOSUB Turn_right
CASE %110· 'WALL and LEFT open RIGHT detect Test OK
LOW 11:LOW 10:HIGH 1
GOSUB Turn_Left
CASE %111 'Wall open LEFT open· RIGHT open Test OK
LOW 11:LOW 10:LOW 1
GOSUB Go_Straight
ENDSELECT
PAUSE 1000
PULSOUT 13,pulseLeft ' Apply the pulse.
PULSOUT 12,pulseRight
LOOP
'++++++Movement subroutines
TURN_RIGHT:
pulseLeft = 850········ 'TURN RIGHT
pulseRight = 850
RETURN
TURN_LEFT:
pulseLeft = 650········ 'TURN LEFT
pulseRight = 650
RETURN
GO_STRAIGHT:
pulseLeft = 850··· 'Go staight
pulseRight = 650
RETURN
BACK_UP:
pulseLeft = 650····· 'BACK UP
pulseRight = 850
RETURN
U_Turn:
·FREQOUT 4, 2000, 6000
·· ' Back up.
FOR pulseCount = 0 TO 40
PULSOUT 13, 650
PULSOUT 12, 850
PAUSE 20
NEXT
FOR pulseCount = 0 TO 20 ' Right turn, about 90-degrees.
PULSOUT 13, 850
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN
Comments
'partial rough code to give you an idea... bounceMax CON 3 'how many times before escape is declared necessary bounceCnt VAR Nib 'count it irBits VAR Nib 'left/right obstacle detection and "last" obs. det. irLeft VAR irBits.BIT0 irRight VAR irBits.BIT1 irLlast VAR irBits.BIT2 irRlast VAR irBits.BIT3 Reset: irBits = 0 'clear 'em -- will be set to "1" on detect Start: Check_for_Obstacles: irLlast = irLeft 'save "last" reading 'insert pulsout to IR led or however you are generating IR irLeft = ~IRdetector 'invert the bit so it's 1 on detect irRlast = irRight 'save "last" 'insert pulsout to IR led or however you are generating IR irRight = ~IRdetector 'invert the bit so it's 1 on detect 'now the lower two bits of irBits are if you have an obstacle -- you can use those with case/select or on...goto 'but first compare with higher two bits -- if the bits all XOR to 1, then you're bouncing IF ( irBits & %0011 ) ^ ( irBits >> 2 ) = %11 THEN bounceCnt = bounceCnt + 1 'you're bouncing, increase count IF bounceCnt >= bounceMax THEN ' 'escape -- an easy trick here would be to artificially set both lower irBits to 1 to "simulate" escape reading irBits = irBits | %0011 ENDIF ELSE bounceCnt = 0 ' not bouncing, reset ENDIF 'read lower two irbits to determine which way to go ON irBits & %0011 GOTO NoObs, obsLeft, obsRight, turnAround obsLeft: 'turn right away from left obs. code GOTO ObsDone obsRight: 'turn left away from right obs. code GOTO ObsDone turnAround: 'turn around code GOTO ObsDone NoObs: ObsDone: 'rest of program GOTO Main▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST