My wall following code is state machine code. I am going forward. The right facing sonar tells me a distance. I compare that to constants and to the previous measurement. Given where I am relative to a line parallel to the wall and whether I am getting closer to or farther from the wall, I steer right, left or straight.
Here's the code:
Code:
keepgoingforward: '----Compare to right wall
IF oldrangeright = rangeright THEN
HPWM 1,Straight,50
ENDIF
IF rangeright > outertrack THEN
HPWM 1,HalfRight,50
ENDIF
IF rangeright < desiredtrack AND oldrangeright < rangeright THEN
HPWM 1,Straight,50
ENDIF
IF rangeright > desiredtrack AND oldrangeright > rangeright THEN
HPWM 1,Straight,50
ENDIF
IF rangeright > desiredtrack AND oldrangeright < rangeright THEN
HPWM 1,HalfRight,50
ENDIF
IF rangeright < desiredtrack AND oldrangeright > rangeright THEN
HPWM 1,HalfLeft,50
ENDIF
GOTO main
Given my limitations this seemed the easiest path to take.
Ken
Bookmarks