Why won't this work?
DarkCrawler
Posts: 24
We recently purchased an Advanced SumoBot, but when we wrote the final competition program, all it did was go around in circles and avoid objects. Could someone tell me what's wrong with this program?
' Sumobot_5.1_Basic_Competition_Program.BS2
' {$STAMP BS2}
' {$PBASIC 2.5}
'
[noparse][[/noparse] I/O Definitions ]
LMotor········· PIN····· 13······················· ' left servo motor
RMotor········· PIN····· 12······················· ' right servo motor
LLinePwr······· PIN····· 10······················· ' left line sensor power
LLineIn········ PIN······ 9······················· ' left line sensor input
RLinePwr······· PIN······ 7······················· ' right line sensor power
RLineIn········ PIN······ 8······················· ' right line sensor input
LfIrOut········ PIN····· 4························ ' left IR LED output
LFIrIn········· PIN····· 11······················· ' left IR sensor input
RtIrOut········ PIN····· 15······················· ' right IR LED output
RtIrIn········· PIN····· 14······················· ' right IR sensor input
Speaker········ PIN······ 1······················· ' piezo speaker
StartLED······· PIN······ 0······················· ' display start delay
'
[noparse][[/noparse] Constants ]
LFwdFast······· CON····· 1000····················· ' left motor fwd; fast
LFwdSlow······· CON······ 800····················· ' left motor fwd; slow
LStop·········· CON······ 750····················· ' left motor stop
LRevSlow······· CON······ 700····················· ' left motor rev; slow
LRevFast······· CON······ 500····················· ' left motor rev; fast
RFwdFast······· CON······ 500····················· ' right motor fwd; fast
RFwdSlow······· CON······ 700····················· ' right motor fwd; slow
RStop·········· CON······ 750····················· ' right motor stop
RRevSlow······· CON······ 800····················· ' right motor rev; slow
RRevFast······· CON····· 1000····················· ' right motor rev; fast
'
[noparse][[/noparse] Variables ]
lLine·········· VAR····· Word····················· ' left sensor raw reading
rline·········· VAR····· Word····················· ' right sensor raw reading
blackThresh···· VAR····· Word····················· ' QTI black threshhold
lineBits······· VAR····· Nib······················ ' decoded sensors value
lbLeft········· VAR····· lineBits.BIT1
lbRight········ VAR····· lineBits.BIT0
irBits········· VAR····· Nib······················ ' IR readings (l & r)
irLeft········· VAR····· irBits.BIT1
irRight········ VAR····· irBits.BIT0
lastIr········· VAR····· Nib······················ ' info from last reading
pulses········· VAR····· Byte····················· ' counter for motor control
temp··········· VAR····· Byte
'
[noparse][[/noparse] EEPROM Data ]
RunStatus······ DATA···· %00······················ ' run status
'
[noparse][[/noparse] Initialization ]
Reset:
· READ RunStatus, temp···························· ' read current status
· temp = ~temp···································· ' invert status
· WRITE RunStatus, temp··························· ' save for next reset
· IF (temp >0) THEN END··························· ' okay to run?
' Sets black threshold to 1/4 the average of the two sensor readings.
' Sumobot must be placed over black playing surface before this code runs.
Set_Threshold:
· GOSUB Read_Line_Sensors
· blackThresh = (lLine / 10) + (rLine / 10)
· LOW LMotor······································ ' make more pins outputs
· LOW RMotor
Start_Delay:······································ ' five second delay
· FOR temp = 1 TO 5
··· HIGH StartLED································· ' show active
··· PAUSE 900
··· INPUT StartLED································ ' blink each second
··· FREQOUT Speaker, 100, 2500, 3000·············· ' beep each second
· NEXT
· GOTO Lunge······································ ' start aggressive!
'
[noparse][[/noparse] Program Code ]
Main:
· GOSUB Read_Line_Sensors
· ' If not on the Shikiri line (border), continue to look for opponent,
· ' otherwise, spin back toward center and resume search
· BRANCH lineBits, [noparse][[/noparse]Search_For_Opponent, Spin_Left, Spin_Right, About_Face]
' --[noparse][[/noparse] Border Avoidence ]--
Spin_Left:········································ ' right sensor was active
· FOR pulses = 1 TO 20
··· PULSOUT LMotor, LRevFast
··· PULSOUT RMotor, RFwdFast
··· PAUSE 20
··· NEXT
··· lastIr = %00·································· ' clear scan direction
··· GOTO Lunge
Spin_Right:
· FOR pulses = 1 TO 20
··· PULSOUT LMotor, LFwdFast
··· PULSOUT RMotor, RRevFast
··· PAUSE 20
··· NEXT
··· lastIr = %00
··· GOTO Lunge
About_Face:······································· ' both sensors on Shikiri
· FOR pulses = 1 TO 10···························· ' back up from edge
··· PULSOUT LMotor, LRevFast
··· PULSOUT RMotor, RRevFast
··· PAUSE 20
· NEXT
· FOR pulses = 1 TO 30···························· ' turn around
··· PULSOUT LMotor, LFwdFast
··· PULSOUT RMotor, RRevFast
··· PAUSE 20
· NEXT
· lastIr = %00
· GOTO Lunge
' --[noparse][[/noparse] IR Processing ]--
Search_For_Opponent:
· GOSUB Read_IR_Sensors
· ' If opponent is not in view, scan last known direction.· Turn toward
· ' opponent if seen by one "eye" -- if both, lunge foward
· BRANCH irBits, [noparse][[/noparse]Scan, Follow_Right, Follow_Left, Lunge]
Scan:
· BRANCH lastIR,· [noparse][[/noparse]Move_Fwd, Scan_Right, Scan_Left]
Move_Fwd:
· GOSUB Creep_Forward
· GOTO Main
Scan_Right:······································· ' spin right, slow
· FOR pulses = 1 TO 5
··· PULSOUT LMotor, LFwdSlow
··· PULSOUT RMotor, RRevSlow
··· PAUSE 20
· NEXT
· GOSUB Creep_Forward····························· ' keep moving
· GOTO Main
Scan_Left:········································ ' spin left, slow
· FOR pulses = 1 TO 5
··· PULSOUT LMotor, LRevSlow
··· PULSOUT RMotor, RFwdSlow
··· PAUSE 20
· NEXT
· GOSUB Creep_Forward
· GOTO Main
Follow_Right:····································· ' spin right, fast
· PULSOUT LMotor, LFwdFast
· PULSOUT RMotor, RRevSlow
· lastIR = irBits································· ' save last direction found
· GOTO Main
Follow_Left:······································ ' spin left, fast
· PULSOUT LMotor, LRevFast
· PULSOUT RMotor, RFwdFast
· lastIR = irBits
· GOTO Main
Lunge:············································ ' locked on -- go get him!
· FOR pulses = 1 TO 25
··· PULSOUT LMotor, LFwdFast
··· PULSOUT RMotor, RFwdFast
··· GOSUB Read_Line_Sensors
··· IF (lineBits = %11) THEN Match_Over··········· ' in sight, we're on the line
· NEXT
· GOTO Main
' If SumoBot can see opponent with both "eyes" and both QTIs are
' detecting the border, then we must have pushed the opponent out.
Match_Over:
· FOR pulses = 1 TO 10
··· PULSOUT LMotor, LSTOP
··· PULSOUT RMotor, RSTOP
··· PAUSE 20
· NEXT
· INPUT LMotor
· INPUT RMotor
· FOR temp = 1 TO 10······························ ' make some noise
··· HIGH StartLED
··· FREQOUT Speaker, 100, 2500, 3000·············· ' beep
··· INPUT StartLED································ ' blink LED
··· PAUSE 100
· NEXT
· DIRS = $0000···································· ' disable all outputs
· GOTO reset······································ ' reset for next round
'
[noparse][[/noparse] Subroutines ]
Read_Line_Sensors:
· HIGH LLinePwr··································· ' activate sensors
· HIGH RLinePwr
· HIGH LLineIn···································· ' discharge caps
· HIGH RLineIn
· PAUSE 1
· RCTIME LLineIn, 1, lLine························ ' read left sensor
· RCTIME RLineIn, 1, rLine························ ' read right sensor
· LOW LLinePwr···································· ' deactivate sensors
· LOW RLinePwr
· ' convert readings to bits
· LOOKDOWN lLine, >=[noparse][[/noparse]1000, 0], lbLeft············· ' 0 = black, 1 = line
· LOOKDOWN rLine, >=[noparse][[/noparse]1000, 0], lbRight
· RETURN
Read_IR_Sensors:
· FREQOUT LfIrOut, 1, 38500······················· ' modulate left IR LED
· irLeft = ~LfIrIn································ ' read input (1 = target)
· FREQOUT RtIROut, 1, 38500························ ' modulate right IR LED
· irRight = ~RtIrIn······························· ' read input (1 = target)
· RETURN
Creep_Forward:
· FOR pulses = 1 TO 20
··· PULSOUT LMotor, LFwdSlow
··· PULSOUT RMotor, RFwdSlow
··· PAUSE 20
· NEXT
· RETURN
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
DrkCrwlr
Post Edited (DarkCrawler) : 7/27/2006 6:46:49 PM GMT
' Sumobot_5.1_Basic_Competition_Program.BS2
' {$STAMP BS2}
' {$PBASIC 2.5}
'
[noparse][[/noparse] I/O Definitions ]
LMotor········· PIN····· 13······················· ' left servo motor
RMotor········· PIN····· 12······················· ' right servo motor
LLinePwr······· PIN····· 10······················· ' left line sensor power
LLineIn········ PIN······ 9······················· ' left line sensor input
RLinePwr······· PIN······ 7······················· ' right line sensor power
RLineIn········ PIN······ 8······················· ' right line sensor input
LfIrOut········ PIN····· 4························ ' left IR LED output
LFIrIn········· PIN····· 11······················· ' left IR sensor input
RtIrOut········ PIN····· 15······················· ' right IR LED output
RtIrIn········· PIN····· 14······················· ' right IR sensor input
Speaker········ PIN······ 1······················· ' piezo speaker
StartLED······· PIN······ 0······················· ' display start delay
'
[noparse][[/noparse] Constants ]
LFwdFast······· CON····· 1000····················· ' left motor fwd; fast
LFwdSlow······· CON······ 800····················· ' left motor fwd; slow
LStop·········· CON······ 750····················· ' left motor stop
LRevSlow······· CON······ 700····················· ' left motor rev; slow
LRevFast······· CON······ 500····················· ' left motor rev; fast
RFwdFast······· CON······ 500····················· ' right motor fwd; fast
RFwdSlow······· CON······ 700····················· ' right motor fwd; slow
RStop·········· CON······ 750····················· ' right motor stop
RRevSlow······· CON······ 800····················· ' right motor rev; slow
RRevFast······· CON····· 1000····················· ' right motor rev; fast
'
[noparse][[/noparse] Variables ]
lLine·········· VAR····· Word····················· ' left sensor raw reading
rline·········· VAR····· Word····················· ' right sensor raw reading
blackThresh···· VAR····· Word····················· ' QTI black threshhold
lineBits······· VAR····· Nib······················ ' decoded sensors value
lbLeft········· VAR····· lineBits.BIT1
lbRight········ VAR····· lineBits.BIT0
irBits········· VAR····· Nib······················ ' IR readings (l & r)
irLeft········· VAR····· irBits.BIT1
irRight········ VAR····· irBits.BIT0
lastIr········· VAR····· Nib······················ ' info from last reading
pulses········· VAR····· Byte····················· ' counter for motor control
temp··········· VAR····· Byte
'
[noparse][[/noparse] EEPROM Data ]
RunStatus······ DATA···· %00······················ ' run status
'
[noparse][[/noparse] Initialization ]
Reset:
· READ RunStatus, temp···························· ' read current status
· temp = ~temp···································· ' invert status
· WRITE RunStatus, temp··························· ' save for next reset
· IF (temp >0) THEN END··························· ' okay to run?
' Sets black threshold to 1/4 the average of the two sensor readings.
' Sumobot must be placed over black playing surface before this code runs.
Set_Threshold:
· GOSUB Read_Line_Sensors
· blackThresh = (lLine / 10) + (rLine / 10)
· LOW LMotor······································ ' make more pins outputs
· LOW RMotor
Start_Delay:······································ ' five second delay
· FOR temp = 1 TO 5
··· HIGH StartLED································· ' show active
··· PAUSE 900
··· INPUT StartLED································ ' blink each second
··· FREQOUT Speaker, 100, 2500, 3000·············· ' beep each second
· NEXT
· GOTO Lunge······································ ' start aggressive!
'
[noparse][[/noparse] Program Code ]
Main:
· GOSUB Read_Line_Sensors
· ' If not on the Shikiri line (border), continue to look for opponent,
· ' otherwise, spin back toward center and resume search
· BRANCH lineBits, [noparse][[/noparse]Search_For_Opponent, Spin_Left, Spin_Right, About_Face]
' --[noparse][[/noparse] Border Avoidence ]--
Spin_Left:········································ ' right sensor was active
· FOR pulses = 1 TO 20
··· PULSOUT LMotor, LRevFast
··· PULSOUT RMotor, RFwdFast
··· PAUSE 20
··· NEXT
··· lastIr = %00·································· ' clear scan direction
··· GOTO Lunge
Spin_Right:
· FOR pulses = 1 TO 20
··· PULSOUT LMotor, LFwdFast
··· PULSOUT RMotor, RRevFast
··· PAUSE 20
··· NEXT
··· lastIr = %00
··· GOTO Lunge
About_Face:······································· ' both sensors on Shikiri
· FOR pulses = 1 TO 10···························· ' back up from edge
··· PULSOUT LMotor, LRevFast
··· PULSOUT RMotor, RRevFast
··· PAUSE 20
· NEXT
· FOR pulses = 1 TO 30···························· ' turn around
··· PULSOUT LMotor, LFwdFast
··· PULSOUT RMotor, RRevFast
··· PAUSE 20
· NEXT
· lastIr = %00
· GOTO Lunge
' --[noparse][[/noparse] IR Processing ]--
Search_For_Opponent:
· GOSUB Read_IR_Sensors
· ' If opponent is not in view, scan last known direction.· Turn toward
· ' opponent if seen by one "eye" -- if both, lunge foward
· BRANCH irBits, [noparse][[/noparse]Scan, Follow_Right, Follow_Left, Lunge]
Scan:
· BRANCH lastIR,· [noparse][[/noparse]Move_Fwd, Scan_Right, Scan_Left]
Move_Fwd:
· GOSUB Creep_Forward
· GOTO Main
Scan_Right:······································· ' spin right, slow
· FOR pulses = 1 TO 5
··· PULSOUT LMotor, LFwdSlow
··· PULSOUT RMotor, RRevSlow
··· PAUSE 20
· NEXT
· GOSUB Creep_Forward····························· ' keep moving
· GOTO Main
Scan_Left:········································ ' spin left, slow
· FOR pulses = 1 TO 5
··· PULSOUT LMotor, LRevSlow
··· PULSOUT RMotor, RFwdSlow
··· PAUSE 20
· NEXT
· GOSUB Creep_Forward
· GOTO Main
Follow_Right:····································· ' spin right, fast
· PULSOUT LMotor, LFwdFast
· PULSOUT RMotor, RRevSlow
· lastIR = irBits································· ' save last direction found
· GOTO Main
Follow_Left:······································ ' spin left, fast
· PULSOUT LMotor, LRevFast
· PULSOUT RMotor, RFwdFast
· lastIR = irBits
· GOTO Main
Lunge:············································ ' locked on -- go get him!
· FOR pulses = 1 TO 25
··· PULSOUT LMotor, LFwdFast
··· PULSOUT RMotor, RFwdFast
··· GOSUB Read_Line_Sensors
··· IF (lineBits = %11) THEN Match_Over··········· ' in sight, we're on the line
· NEXT
· GOTO Main
' If SumoBot can see opponent with both "eyes" and both QTIs are
' detecting the border, then we must have pushed the opponent out.
Match_Over:
· FOR pulses = 1 TO 10
··· PULSOUT LMotor, LSTOP
··· PULSOUT RMotor, RSTOP
··· PAUSE 20
· NEXT
· INPUT LMotor
· INPUT RMotor
· FOR temp = 1 TO 10······························ ' make some noise
··· HIGH StartLED
··· FREQOUT Speaker, 100, 2500, 3000·············· ' beep
··· INPUT StartLED································ ' blink LED
··· PAUSE 100
· NEXT
· DIRS = $0000···································· ' disable all outputs
· GOTO reset······································ ' reset for next round
'
[noparse][[/noparse] Subroutines ]
Read_Line_Sensors:
· HIGH LLinePwr··································· ' activate sensors
· HIGH RLinePwr
· HIGH LLineIn···································· ' discharge caps
· HIGH RLineIn
· PAUSE 1
· RCTIME LLineIn, 1, lLine························ ' read left sensor
· RCTIME RLineIn, 1, rLine························ ' read right sensor
· LOW LLinePwr···································· ' deactivate sensors
· LOW RLinePwr
· ' convert readings to bits
· LOOKDOWN lLine, >=[noparse][[/noparse]1000, 0], lbLeft············· ' 0 = black, 1 = line
· LOOKDOWN rLine, >=[noparse][[/noparse]1000, 0], lbRight
· RETURN
Read_IR_Sensors:
· FREQOUT LfIrOut, 1, 38500······················· ' modulate left IR LED
· irLeft = ~LfIrIn································ ' read input (1 = target)
· FREQOUT RtIROut, 1, 38500························ ' modulate right IR LED
· irRight = ~RtIrIn······························· ' read input (1 = target)
· RETURN
Creep_Forward:
· FOR pulses = 1 TO 20
··· PULSOUT LMotor, LFwdSlow
··· PULSOUT RMotor, RFwdSlow
··· PAUSE 20
· NEXT
· RETURN
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
DrkCrwlr
Post Edited (DarkCrawler) : 7/27/2006 6:46:49 PM GMT
Comments
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
DrkCrwlr