|
This was the core operation program
within the robot we constructed. This source code gave the robot the
ability to think and act on it's own, and told the robot how to transmit
the data back to the laptop we used to map the data coming from it's
sensors.
The reason why we used PBasic is
because that is the programming language that can only be used when
programming the Basic Stamp. When the Basic Stamp program is downloaded
into memory, the PBasic code gets converted to Assembly language since
the EEPROM can only understand ASM.
'{$STAMP BS2p}
'{$PORT COM1}
'{$PBASIC 2.5}
'Really 1337 robot control program
'By Paul R and Matt F
'
'(C) 2003
'-----Define LCD constants-----
WakeUp
CON
%00110000 'Wake-up
FourBitMode
CON %00100000 'Set to 4-bit mode
OneLine5x8Font
CON %00100000 'Set to 1 display line, 5x8 font
OneLine5x10Font
CON %00100100 'Set to 1 display line, 5x10 font
TwoLine5x8Font
CON %00101000 'Set to 2 display lines, 5x8 font
TwoLine5x10Font
CON %00101100 'Set to 2 display lines, 5x10 font
DisplayOff CON
%00001000 'Turn off display, data is retained
DisplayOn CON
%00001100 'Turn on display, no cursor
DisplayOnULCrsr
CON %00001110 'Turn on display, with underline cursor
DisplayOnBLCrsr
CON %00001101 'Turn on display, with blinking cursor
IncCrsr CON
%00000110 'Auto-increment cursor, no display shift
IncCrsrShift CON
%00000111 'Auto-increment cursor, shift display left
DecCrsr CON
%00000100 'Auto-decrement cursor, no display shift
DecCrsrShift CON
%00000101 'Auto-decrement cursor, shift display right
ClearDisplay CON
%00000001 'Clear the display
HomeDisplay CON
%00000010 'Move cursor and display to home position
ScrollLeft CON
%00011000 'Scroll display to the left
ScrollRight CON
%00011100 'Scroll display to the right
CrsrLeft CON
%00010000 'Move cursor left
CrsrRight CON
%00010100 'Move cursor right
MoveCrsr CON
%10000000 'Move cursor to position (must add address)
MoveToCGRAM CON
%01000000 'Move to CGRAM position (must add address)
'------------------------------
'-- Variable Settings --
wrdLoopVal
VAR WORD 'Defined value for all for loops
sDistanceR
VAR WORD 'Defined value to store distance from the right sonar sensor
sDistanceC
VAR WORD 'Defined value to store distance from the center sonar sensor
sDistanceL
VAR WORD 'Defined value to store distance from the left sonar sensor
bytSonarR
VAR BYTE 'Defined value to store switch from the right sonar sensor
bytSonarC
VAR BYTE 'Defined value to store switch from the center sonar sensor
bytSonarL
VAR BYTE 'Defined value to store switch from the left sonar sensor
IrodLval
VAR BYTE 'Defined value to store switch from the left IROD
IrodRval
VAR BYTE 'Defined value to store switch from the right IROD
bytSerData
VAR BYTE 'Defined value to store data from the transceivers
SonarDist
CON 75 '75 cm for minimum sonar switching distance
'------------------------------
'-- Defined Stamp Pins --
SonarR
CON 10 'Pulse receiving pin for right sonar sensor
SonarC
CON 11 'Pulse receiving pin for center sonar sensor
SonarL
CON 12 'Pulse receiving pin for left sonar sensor
SonarP
CON 13 'Pulse transmitting pin for all sonar sensor
IrodL
CON 14 'Infrared detection pin for left IROD
IrodR
CON 15 'Infrared detection pin for right IROD
TXpin
CON 9 'Transceiver pin for transmitting serial signals
RXpin
CON 8 'Transceiver pin for receiving serial signals
'------------------------------
'-- Basic Stamp Pin Switch Settings --
MAINIO 'Switch to the Main I/O BS2P40 stamp pins (1-15)
INPUT SonarR 'Declare the pin that the right sonar sensor uses as input
INPUT SonarC 'Declare the pin that the center sonar sensor uses as input
INPUT SonarL 'Declare the pin that the left sonar sensor uses as input
INPUT IrodL 'Declare the pin that the left IROD sensor uses as input
INPUT IrodR 'Declare the pin that the right IROD sensor uses as input
OUT13=0 'Declare the pin that all sonar sensors use to transmit as output
AUXIO 'Switch to the Aux I/0 BS2P40 stamp pins (16-30)
OUT15=0 'Declare motor control pin as output
OUT14=0 'Declare motor control pin as output
OUT13=0 'Declare motor control pin as output
OUT12=0 'Declare motor control pin as output
MAINIO 'Switch to the Main I/O BS2P40 stamp pin (1-15)
'-------------------------------
'-- Main Program
Startup:
'Load display settings to the LCD before starting up
GOSUB
InitLCD
LCDOUT 0,HomeDisplay,["Really 1337
Bot"]
LCDOUT
0,MoveCrsr+64,["Initializing...."]
PAUSE
1000
mLoop: 'Never stop the program if everything is normal
'Check to see if the external push button has been pressed
GOSUB
CheckButton
'If the program is running, always gather distances from the
'the sonar sensors, and on/off status from the IROD's
GOSUB
getSonarR
GOSUB
getSonarC
GOSUB
getSonarL
GOSUB
CheckIrods
'Display the switching status of all the sonar sensors and the IROD sensors
'to the LCD screen
LCDOUT 0, ClearDisplay,["SR:",DEC
bytSonarR," SC:",DEC bytSonarC," SL:",DEC bytSonarL]
LCDOUT
0, MoveCrsr+64,
["IRR:",DEC IrodRval," IRL:",DEC IrodLval]
'If all sonar switches are off then...
IF
bytSonarL = 0
AND bytSonarC = 0
AND bytSonarR = 0
THEN
GOSUB
BotForward 'Drive robot in forward position
ENDIF
'If sonar left switch is on and the rest are off then...
IF
bytSonarL = 1
AND bytSonarC = 0
AND bytSonarR = 0
THEN
GOSUB
BotRight 'Turn robot to the right
PAUSE
2000 'Wait 2 seconds
GOSUB
GetSensors 'Gather info from the sensors
GOSUB
BotForward 'Drive robot in forward position
ENDIF
'If sonar left, center switch are on, and the rest off then...
IF
bytSonarL = 1
AND bytSonarC = 1
AND bytSonarR = 0
THEN
GOSUB
BotRight 'Turn robot to the right
PAUSE
3000 'Wait 3 seconds
GOSUB
GetSensors 'Gather info from the sensors
GOSUB
BotForward 'Drive robot in forward position
FOR
wrdLoopVal = 1
TO 3
: NEXT 'Wait 3 milliseconds
ENDIF
'If sonar right is on and the rest off then...
IF
bytSonarL = 0
AND bytSonarC = 0
AND bytSonarR = 1
THEN
GOSUB
BotLeft 'Turn the robot to the left
PAUSE
2000 'Wait 2 seconds
GOSUB
GetSensors 'Gather info from the sensors
GOSUB
BotForward 'Drive robot in the forward position
ENDIF
'If sonar right is on and the rest off then...
IF
bytSonarL = 0
AND bytSonarC = 1
AND bytSonarR = 1
THEN
GOSUB
BotLeft 'Turn the robot to the left
PAUSE
3000 'Wait 3 seconds
GOSUB
GetSensors 'Gather info from the sensors
GOSUB
BotForward 'Drive robot in the forward position
FOR
wrdLoopVal = 1
TO 3
: NEXT 'Wait 3 milliseconds
ENDIF
'If all sonar sensors are on then...
IF
bytSonarL = 1
AND bytSonarC = 1
AND bytSonarR = 1
THEN
GOSUB
BotBackward 'Drive robot in the backward position
PAUSE
2000 'Wait 2 seconds
'Go backwards and loop for 2 seconds while checking for obsticles
GOSUB
GetSensors
'If the IROD sensors come on then...
IF
IrodRval = 1
OR IrodLval = 1
THEN
'IRODS detected something. Dead stop.
GOSUB
BotKillStop
GOSUB
HelpStuck
ENDIF
GOSUB
BotRight
'Truen robot right
'Loop for 2 secs and keep checking so we don't run into anything
FOR
wrdLoopVal = 1
TO 2000 'Loop 2000 1 ms sequences
PAUSE
1
NEXT
ENDIF
PAUSE
20 :
SEROUT
9,16624,[30]
PAUSE
20 :
SEROUT
9,16624,[sDistanceL] 'Transmit left sonar distance
PAUSE
20 :
SEROUT
9,16624,[254]
PAUSE 20 :
SEROUT 9,16624,[31]
PAUSE
20 :
SEROUT
9,16624,[sDistanceC] 'Transmit center sonar distance
PAUSE
20 :
SEROUT
9,16624,[254]
PAUSE
20 :
SEROUT
9,16624,[32]
PAUSE
20 :
SEROUT
9,16624,[sDistanceR] 'Transmit right sonar distance
PAUSE
20 :
SEROUT
9,16624,[254]
PAUSE
20 :
SEROUT
9,16624,[33]
PAUSE
20 :
SEROUT
9,16624,[IrodLval] 'Transmit left IROD sensor status
PAUSE
20 :
SEROUT
9,16624,[254]
PAUSE 20 :
SEROUT
9,16624,[34]
PAUSE
20 :
SEROUT
9,16624,[IrodRval] 'Transmit right IROD sensor status
PAUSE
20 :
SEROUT
9,16624,[254]
PAUSE
20
GOTO
mLoop
'-------------------------------
'-- Start the LCD --
InitLCD:
MAINIO 'Switch to the Main I/O BS2P40 stamp pins (1-15)
LCDCMD
0,WakeUp 'Send wakeup sequence to LCD
PAUSE
10 'These pauses are necessary to meet the LCD specs
LCDCMD
0,WakeUp
PAUSE
1
LCDCMD
0,WakeUp
PAUSE
1
LCDCMD
0,FourBitMode 'Set bus to 4-bit mode
LCDCMD
0,TwoLine5x8Font 'Set to 2-line mode with 5x8 font
LCDCMD
0,DisplayOff 'Turn display off
LCDCMD
0,DisplayOn 'Turn display on with blinking cursor
LCDCMD
0,ClearDisplay 'Clear the display
RETURN
'-------------------------------
'-- Get distance from Sonar Right --
getSonarR:
PULSOUT
SonarP, 5 'Send a pulse out to sensor at 5ms
PULSIN
SonarR,1,sDistanceR 'Collect and report echo pulse
sDistanceR =
sDistanceR / 29 'Increment pulses
PAUSE
10 'Wait 10 milliseconds
'If distance is less then the switching constant then
'set switch as on. If not then set the switch as off
IF
sDistanceR <= SonarDist
THEN
bytSonarR = 1
ELSE
bytSonarR = 0
ENDIF
RETURN
'-------------------------------
'-- Get distance from Sonar Center --
getSonarC:
PULSOUT
SonarP, 5 'Send a pulse out to sensor at 5ms
PULSIN
SonarC,1,sDistanceC 'Collect and report echo pulse
sDistanceC =
sDistanceC / 29 'Increment pulses
PAUSE
10 'Wait 10 milliseconds
'If distance is less then the switching constant then
'set switch as on. If not then set switch as off
IF sDistanceC <= SonarDist
THEN
bytSonarC = 1
ELSE
bytSonarC = 0
ENDIF
RETURN
'-------------------------------
'-- Get distance from Sonar Left --
getSonarL:
PULSOUT
SonarP, 5 'Send a pulse out to sensor at 5ms
PULSIN
SonarL,1,sDistanceL 'Collect and report echo pulse
sDistanceL = sDistanceL / 29 'Increment pulses
PAUSE
10 'Wait 10 milliseconds
'If distance is less then the switching constant then
'set switch as on. If not then set switch as off
IF
sDistanceL <= SonarDist
THEN
bytSonarL = 1
ELSE
bytSonarL = 0
ENDIF
RETURN
'-------------------------------
'-- Transceiver Handler --
GetSensors:
GOSUB
BotKillstop 'Hard mechanical stop
GOSUB
getSonarR 'Collect data from right sonar sensor
GOSUB
getSonarC 'Collect data from center sonar sensor
GOSUB
getSonarL 'Collect data from left sonar sensor
GOSUB
CheckIrods 'Collect data from IROD sensors
PAUSE
20 :
SEROUT
9,16624,[30]
PAUSE
20 :
SEROUT
9,16624,[sDistanceL]
'Transmit left sonar distance
PAUSE
20 :
SEROUT
9,16624,[254]
PAUSE
20 :
SEROUT
9,16624,[31]
PAUSE
20 :
SEROUT
9,16624,[sDistanceC] 'Transmit center sonar distance
PAUSE
20 :
SEROUT
9,16624,[254]
PAUSE
20 :
SEROUT 9,16624,[32]
PAUSE
20 :
SEROUT 9,16624,[sDistanceR]
'Transmit right sonar distance
PAUSE
20 :
SEROUT
9,16624,[254]
PAUSE 20 :
SEROUT
9,16624,[33]
PAUSE
20 :
SEROUT
9,16624,[IrodLval] 'Transmit left IROD sensor status
PAUSE 20 :
SEROUT
9,16624,[254]
PAUSE 20 :
SEROUT
9,16624,[34]
PAUSE 20 :
SEROUT
9,16624,[IrodRval] 'Transmit right IROD sensor status
PAUSE
20 :
SEROUT
9,16624,[254]
'Tell host stamp that we want to recieve data then wait for a reply
SERIN 8,16624,3000,NoSerData,[bytSerData]
'Handle ending process
IF
bytSerData = 1
THEN GOSUB
EndCommand
IF
bytSerData = 2
THEN GOSUB
BotBack2s
IF
bytSerData = 3
THEN GOSUB
Right2s
IF
bytSerData = 4
THEN GOSUB
Left2s
NoSerData:
RETURN
'-------------------------------
'-- Move back for 2 seconds --
BotBack2s:
GOSUB
Botbackward 'Drive robot in backward position
PAUSE
2000 'Wait 2 seconds
GOSUB
GetSensors 'Gather data from sensors
RETURN
'-------------------------------
'-- Move right for 2 seconds --
Right2s:
GOSUB
BotRight 'Turn the robot right
PAUSE
2000 'Wait 2 seconds
GOSUB
GetSensors 'Gather data from sensors
RETURN
'-------------------------------
'-- Move left for 2 seconds --
Left2s:
GOSUB
BotLeft 'Turn the robot left
PAUSE
2000 'Wait 2 seconds
GOSUB
GetSensors 'Gather data from sensors
RETURN
'-------------------------------
'-- IROD right value high --
IrodRhigh:
IrodRVal = 0 'Set low byte
RETURN
'-------------------------------
'-- IROD left value high --
IrodLhigh:
IrodLVal = 0 'Set low byte
RETURN
'-------------------------------
'-- IROD right value low --
IrodRlow:
IrodRVal = 1 'Set high byte
RETURN
'-------------------------------
'-- IROD left value low --
IrodLlow:
IrodLVal = 1 'Set high byte
RETURN
'-------------------------------
'-- Hard Mechanical stop --
BotKillStop:
PAUSE
20 :
SEROUT
9,16624,[4] 'Transmit motor status
PAUSE
20 :
SEROUT
9,16624,[1] 'Transmit motor status
PAUSE
20 : SEROUT
9,16624,[254] 'Transmit motor status
PAUSE
20 :
GOSUB
BotForward 'Drive robot in forward position
PAUSE
100 :
GOSUB
BotBackward 'Drive robot in backward position
PAUSE
100 :
GOSUB
BotStop 'Stop robot motors
RETURN
'-------------------------------
'-- Robot Stop --
BotStop:
PAUSE
20 :
SEROUT
9,16624,[4] 'Transmit motor status
PAUSE
20 : SEROUT
9,16624,[1] 'Transmit motor status
PAUSE
20 : SEROUT
9,16624,[254] 'Transmit motor status
PAUSE
20 'Wait 20 milliseconds
AUXIO 'Switch to the Aux I/O BS2P40 stamp pins (16-30)
HIGH
15 'Motor control pin is off
HIGH
14 'Motor control pin is off
HIGH
13 'Motor control pin is off
HIGH
12 'Motor control pin is off
MAINIO 'Switch to the Main I/O BS2P40 stamp pins (1-15)
RETURN
'-------------------------------
'-- Robot Backward --
BotBackward:
PAUSE 20 :
SEROUT 9,16624,[4] 'Transmit motor status
PAUSE
20 : SEROUT
9,16624,[2] 'Transmit motor status
PAUSE
20 :
SEROUT
9,16624,[254] 'Transmit motor status
PAUSE
20 'Wait 20 milliseconds
AUXIO 'Switch to the Aux I/O BS2P40 stamp pins (16-30)
LOW
15 'Motor control pin is on
HIGH
14 'Motor control pin is off
LOW
13 'Motor control pin is on
HIGH
12 'Motor control pin is off
MAINIO 'Switch to the Main I/O BS2P40 stamp pins (1-15)
RETURN
'-------------------------------
'-- Robot Forward --
BotForward:
PAUSE
20 :
SEROUT
9,16624,[4] 'Transmit motor status
PAUSE
20 :
SEROUT
9,16624,[3] 'Transmit motor status
PAUSE
20 :
SEROUT 9,16624,[254] 'Transmit motor status
PAUSE
20 'Wait 20 milliseconds
AUXIO 'Switch to the Aux I/O BS2P40 stamp pins (16-30)
LOW
15 'Motor control pin is on
LOW
14 'Motor control pin is on
LOW
13 'Motor control pin is on
LOW
12 'Motor control pin is on
MAINIO 'Switch to the Main I/O BS2P40 stamp pins (1-15)
RETURN
'-------------------------------
'-- Robot Right --
BotRight:
PAUSE
20 :
SEROUT
9,16624,[4] 'Transmit motor status
PAUSE
20 :
SEROUT
9,16624,[4] 'Transmit motor status
PAUSE
20 :
SEROUT
9,16624,[254] 'Transmit motor status
PAUSE
20 'Wait 20 milliseconds
AUXIO 'Switch to the Aux I/O BS2P40 stamp pins (16-30)
LOW
15 'Motor control pin is on
HIGH
14 'Motor control pin is off
LOW
13 'Motor control pin is on
LOW
12 'Motor control pin is on
MAINIO 'Switch to the Main I/O BS2P40 stamp pins (1-15)
RETURN
'-------------------------------
'-- Robot Right --
BotLeft:
PAUSE
20 : SEROUT
9,16624,[4] 'Transmit motor status
PAUSE
20 : SEROUT
9,16624,[5] 'Transmit motor status
PAUSE
20 :
SEROUT
9,16624,[254] 'Transmit Motor status
PAUSE
20 'Wait 20 milliseconds
AUXIO 'Switch to the Aux I/O BS2P40 stamp pins (16-30)
LOW
15 'Motor control pin is on
LOW
14 'Motor control pin is on
LOW
13 'Motor control pin is on
HIGH
12 'Motor control pin is off
MAINIO 'Switch to the Main I/O BS2P40 stamp pins (1-15)
RETURN
'-------------------------------
'-- Check IROD Status --
CheckIrods:
'If IROD left is on, report back high, else report back low
IF
IN14 =
1
THEN
GOSUB
IrodLhigh
ELSE
GOSUB
IrodLlow
ENDIF
'If IROD right is on, report back high, else report back low
IF IN15
= 1
THEN
GOSUB
IrodRhigh
ELSE
GOSUB
IrodRlow
ENDIF
RETURN
'-------------------------------
'-- Robot is stuck in a corner --
HelpStuck:
GOSUB
BotStop 'Stop robot
'Display the robots situation on the LCD screen
LCDOUT
0,ClearDisplay,["Help! I'm stuck"]
LCDOUT
0,MoveCrsr+64,["Program Ended"]
PAUSE
20 : SEROUT
9,16624,[3] 'Transmit robot status
PAUSE
20 : SEROUT
9,16624,[250] 'Transmit robot status
PAUSE
20 :
SEROUT
9,16624,[254] 'Transmit robot status
PAUSE 20 :
END 'End the program
RETURN
'-------------------------------
'-- Remote control stop --
EndCommand:
GOSUB
BotStop
'Display the robots situation on the LCD screen
LCDOUT
0,ClearDisplay,["Recieved Kill"]
LCDOUT
0,MoveCrsr+64,["Program Ended"]
PAUSE
20 : SEROUT
9,16624,[3] 'Transmit robot status
PAUSE
20 :
SEROUT
9,16624,[250] 'Transmit robot status
PAUSE
20 :
SEROUT
9,16624,[254] 'Transmit robot status
PAUSE
20 :
END 'End the program
RETURN
'-------------------------------
'-- Check safety stop --
CheckButton:
AUXIO 'Switch to the Aux I/O BS2P40 stamp pins (1-15)
INPUT
11 'Declare input pin for switch
IF IN11 = 0
THEN
MAINIO 'Switch to the Main I/O BS2P40 stamp pins (1-15)
'Display the robots situation on the LCD screen
LCDOUT
0,ClearDisplay,["Motors Stop!"]
LCDOUT
0,MoveCrsr+64,["Finished."]
GOSUB
BotStop 'Stop robot
PAUSE
20 :
SEROUT
9,16624,[3] 'Transmit robot status
PAUSE
20 :
SEROUT
9,16624,[250] 'Transmit robot status
PAUSE
20 : SEROUT
9,16624,[254] 'Transmit robot status
PAUSE
20 :
END 'End the program
ENDIF
MAINIO 'Switch to the Main I/O BS2P40 stamp pins (1-15)
RETURN
'-------------------------------
|
