Mark 3: May I Go? Code

By Benbo231
@ April 13, 2009

• Actuators / output devices: 2 servos
• Control method: Full autonomous
• CPU: Basic Stamp 2
• Operating system: Basic
• Power source: 4 AA batteries
• Programming language: Basic
• Sensors / input devices: Ping))) Ultra Sound
• Target environment: Indoors is best

He will never be complete, as he is a simple test platform I use for testing new code ideas I have. It doesn't take much for him to be bumped up a number. As oddbot says,  defective robots seem to have personality, and Mark seems to have a pretty innocent, accepting one, though at some times he can be quite mean (code not responding to two stimuli that rarely occur), but hopefully, with some therapy he can be fixed.

Mark 3 is my first robot. His brains consist of a Basic Stamp 2 on a Board Of Education. He has 2 continuous rotation drive servos, and one 'neck' servo. His only sensor is a PING))) ultrasonic rangefinder.

Basically what he does is drive forward, looking left, right, and straight ahead, until he sees anything closer then about a foot away. If he sees while his head is on the left, he turns right. If he sees it when his head is turned right, he turns left. If his head is facing forward, he backs up and turns 180 degrees.

I have posted my first video on LMR! Yay!

' {$STAMP BS2}
' {$PBASIC 2.5}
Ping PIN 15
rawDist VAR Word
leftDist VAR Word
HleftDist VAR Word
rightDist VAR Word
HrightDist VAR Word
BestDist VAR Word

BestRoute VAR Byte
cnt VAR Byte
trigger CON 5
IsHigh CON 1
IsLow CON 0
Scale CON 200

ROOT:
BestDist = 0
FOR cnt = 1 TO 15 'Both Head and Robot go forward
PULSOUT 12, 500
PULSOUT 13, 1000
PULSOUT 14, 750
PAUSE 25
NEXT
GOSUB Get_Sonar ' Center Distance Reading
PAUSE 25
IF rawDist < 300 THEN GOSUB Front
IF rawDist < 300 THEN GOSUB Scan
FOR cnt = 1 TO 15 'Head turns left, Robot goes forward
PULSOUT 12, 500
PULSOUT 13, 1000
PULSOUT 14, 1000
PAUSE 25
NEXT
GOSUB LeftPing ' Left Distance Reading
PAUSE 25
IF leftDist < 300 THEN GOSUB Left
IF leftDist < 300 THEN GOSUB Scan
FOR cnt = 1 TO 15 'Both Head and Robot go forward
PULSOUT 12, 500
PULSOUT 13, 1000
PULSOUT 14, 750
PAUSE 25
NEXT
GOSUB Get_Sonar ' Center Distance Reading
PAUSE 25
IF rawDist < 300 THEN GOSUB Front
IF rawDist < 300 THEN GOSUB Scan
FOR cnt = 1 TO 15 'Head goes right, robot goes forward
PULSOUT 12, 500
PULSOUT 13, 1000
PULSOUT 14, 500
PAUSE 25   'Servo Cooloff
NEXT
GOSUB RightPing ' Right Distance Reading
PAUSE 25
IF rightDist < 300 THEN GOSUB Right
IF rightDist < 300 THEN GOSUB Scan
GOTO ROOT 'loops to top
END

Get_Sonar:
Ping = IsLow ' make trigger 0-1-0
PULSOUT Ping, Trigger ' activate sensor
PULSIN Ping, IsHigh, rawDist ' measure echo pulse
rawDist = rawDist */ Scale ' convert to uS
rawDist = rawDist / 2 ' remove return trip
PAUSE 20
RETURN

LeftPing:
Ping = IsLow ' make trigger 0-1-0
PULSOUT Ping, Trigger ' activate sensor
PULSIN Ping, IsHigh, leftDist ' measure echo pulse
leftDist = leftDist */ Scale ' convert to uS
leftDist = leftDist / 2 ' remove return trip
PAUSE 20
RETURN

HalfleftPing:
Ping = IsLow ' make trigger 0-1-0
PULSOUT Ping, Trigger ' activate sensor
PULSIN Ping, IsHigh, HleftDist ' measure echo pulse
HleftDist = HleftDist */ Scale ' convert to uS
HleftDist = HleftDist / 2 ' remove return trip
PAUSE 20
RETURN

RightPing:
Ping = IsLow ' make trigger 0-1-0
PULSOUT Ping, Trigger ' activate sensor
PULSIN Ping, IsHigh, rightDist ' measure echo pulse
rightDist = rightDist */ Scale ' convert to uS
rightDist = rightDist / 2 ' remove return trip
PAUSE 20
RETURN

HalfrightPing:
Ping = IsLow ' make trigger 0-1-0
PULSOUT Ping, Trigger ' activate sensor
PULSIN Ping, IsHigh, HrightDist ' measure echo pulse
HrightDist = HrightDist */ Scale ' convert to uS
HrightDist = HrightDist / 2 ' remove return trip
PAUSE 20
RETURN

Right: 'If it sees something while the head is turned right, it turns left
DO
PULSOUT 12, 500
PULSOUT 13, 500
PAUSE 25
GOSUB LeftPing
LOOP UNTIL leftDist > 300
RETURN

Left: 'If it sees something while the head is turned left, it turns right
DO
PULSOUT 12, 1000
PULSOUT 13, 1000
PAUSE 25
GOSUB rightPing
LOOP UNTIL rightDist > 300
RETURN

Front: 'If it sees something while the head is forward, it backs up and turns left
DO
PULSOUT 12, 1000   '
PULSOUT 13, 500
PAUSE 25
GOSUB Get_Sonar
LOOP UNTIL rawDist > 300
FOR cnt = 1 TO 65
PULSOUT 12, 500
PULSOUT 13, 500
PAUSE 25
NEXT
RETURN

Scan:                 'This is where C.T.C.'s May I Go? Navigation starts.
FOR Cnt = 1 TO 10     'Center Eyes
PULSOUT 14, 750
PAUSE 25
NEXT
GOSUB Get_Sonar       'Center Sonar
FOR Cnt = 1 TO 10     'Eyes hard to left, 45 degrees
PULSOUT 14, 1000
PAUSE  25
NEXT
GOSUB leftPing        'Hard Left Sonar
FOR Cnt = 1 TO 10     'Eyes to 22.5 left
PULSOUT 14, 875
PAUSE 25
NEXT
GOSUB HalfleftPing    'Soft Left Sonar
FOR Cnt = 1 TO 10     'Center Eyes
PULSOUT 14, 750
PAUSE 25
NEXT                  'No sonar needed, did it up top.
FOR Cnt = 1 TO 10     'Eyes to 22.5 right
PULSOUT 14, 625
PAUSE 25
NEXT
GOSUB HalfrightPing   'Soft Right Sonar
FOR Cnt = 1 TO 10     'Eyes hard to right, 45 degrees
PULSOUT 14, 500
PAUSE 25
NEXT
GOSUB RightPing       'Hard Right Sonar
FOR Cnt = 1 TO 10     'Center Eyes
PULSOUT 14, 750
PAUSE 25
NEXT
'Here comes the math!
IF leftDist > BestDist THEN BestRoute = 1           'Sets the best path as One. It knows One is turn 45 degrees left.
IF leftDist > BestDist THEN BestDist = leftDist     'Sets the bar at the distance reading of 45 degrees left.
IF HleftDist > BestDist THEN BestRoute = 2          'Sets the best path as Two. It knows Two is turn 22.5 degrees left.
IF HleftDist > BestDist THEN BestDist = HleftDist   'If 22.5 degrees left is farther then the previous reading, set the bar higher. Otherwise, Throw it away.
IF rawDist > BestDist THEN BestRoute = 3            'Sets the best path as Three. It knows Three is go straight.
IF rawDist > BestDist THEN BestDist = rawDist       'If 0 Degrees is farther then the previous reading, set the bar higher.
IF HrightDist > BestDist THEN BestRoute = 4         'Sets the best path as Four. It knows four is turn 22.5 degrees right.
IF HrightDist > BestDist THEN BestDist = HrightDist 'If 22.5 degrees right is farther then the previous reading, set the bar higher.
IF rightDist > BestDist THEN BestRoute = 5          'Sets the best path as Five. It knows five is turn 45 degrees right.
IF rightDist > BestDist THEN BestDist = rightDist   'If 45 degrees right is farther then the previous reading, set the bar higher.
'Find which way I decided is best, and execute it.
IF BestRoute = 1 THEN GOSUB Hardleft
IF BestRoute = 2 THEN GOSUB Softleft
IF BestRoute = 3 THEN RETURN
IF BestRoute = 4 THEN GOSUB SoftRight
IF BestRoute = 5 THEN GOSUB HardRight
RETURN

HardLeft:
FOR cnt = 1 TO 22
PULSOUT 12, 500
PULSOUT 13, 500
PAUSE 25
NEXT
RETURN

SoftLeft:
FOR cnt = 1 TO 11
PULSOUT 12, 500
PULSOUT 13, 500
PAUSE 25
NEXT
RETURN

SoftRight:
FOR cnt = 1 TO 11
PULSOUT 12, 1000
PULSOUT 13, 1000
PAUSE 25
NEXT
RETURN

HardRight:
FOR cnt = 1 TO 22
PULSOUT 12, 1000
PULSOUT 13, 1000
PAUSE 25
NEXT
RETURn