Sumobot Code: side IR's not working
Can anyone please tell me why my side IR's are not working. I'm a new poster and we were successful up until we tried to get the side sensors added in. Thanks
' SumoBot-5.1-Basic-Competition-Program.BS2
' {$STAMP BS2}
' {$PBASIC 2.5}
'
[ 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
IrLedLS PIN 2 ' Left IR LED connected to P2
IrSenseLS PIN 1 ' Left IR detector to P1
IrLedRS PIN 3 ' Right IR LED connected to P3
IrSenseRS PIN 0 ' Right IR detector to P0
ledspeaker PIN 5 ' piezo speaker
Startled PIN 0
'
[ 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
'
[ Variables ]
lLine VAR Word ' left sensor raw reading
rLine VAR Word ' right sensor raw reading
blackThresh VAR Word ' QTI black threshold
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
irLSide VAR irBits.BIT2 ' IR readings (l & r sides)
irRSide VAR irBits.BIT3
pulses VAR Byte ' counter for motor control
temp VAR Byte
'
[ EEPROM Data ]
RunStatus DATA $00 ' run status
'
[ 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: ' set QTI black 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 ledspeaker, 100, 2500, 3000 ' beep each second
NEXT
GOTO Lunge ' start aggressive!
'
[ 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, [Search_For_Opponent, Spin_Left, Spin_Right, About_Face]
' --[ Border Avoidance ]--
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: ' left sensor was active
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
' --[ 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 forward
BRANCH irBits, [Scan, Follow_Right, Follow_Left, Lunge]
Scan:
BRANCH lastIR, [Move_Fwd, Scan_Right, Scan_Left]
Move_Fwd:
FOR pulses = 1 TO 40
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RFwdFast
NEXT
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, LRevSlow
PULSOUT RMotor, RFwdFast
lastIR = irBits
GOTO Main
Lunge: ' locked on -- go get him!
FOR pulses = 1 TO 40
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RFwdFast
NEXT
GOTO Main
' If SumoBot can see the opponent with both "eyes" and both QTIs are
' detecting the border, we must have pushed the opponent out.
'
[ 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, >=[500, 0], lbLeft ' 0 = black, 1 = line
LOOKDOWN rLine, >=[600, 0], lbRight
RETURN
Read_IR_Sensors:
FREQOUT LfIrOut, 1, 38000 ' modulate left IR LED
irLeft = ~LfIrIn ' read input (1 = target)
FREQOUT RtIrOut, 1, 38000 ' modulate right IR LED
irRight = ~RtIrIn ' read input (1 = target)
FREQOUT IrLedLS, 1, 38000 ' modulate lefts IR LED
irLSide = ~irLSide ' read input (1 = target)
FREQOUT IrLedRS, 1, 38000 ' modulate rights IR LED
irRSide = ~irRSide ' read input (1 = target)
RETURN
Creep_Forward:
FOR pulses = 1 TO 20
PULSOUT LMotor, LFwdSlow
PULSOUT RMotor, RFwdSlow
PAUSE 20
NEXT
RETURN
' SumoBot-5.1-Basic-Competition-Program.BS2
' {$STAMP BS2}
' {$PBASIC 2.5}
'
[ 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
IrLedLS PIN 2 ' Left IR LED connected to P2
IrSenseLS PIN 1 ' Left IR detector to P1
IrLedRS PIN 3 ' Right IR LED connected to P3
IrSenseRS PIN 0 ' Right IR detector to P0
ledspeaker PIN 5 ' piezo speaker
Startled PIN 0
'
[ 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
'
[ Variables ]
lLine VAR Word ' left sensor raw reading
rLine VAR Word ' right sensor raw reading
blackThresh VAR Word ' QTI black threshold
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
irLSide VAR irBits.BIT2 ' IR readings (l & r sides)
irRSide VAR irBits.BIT3
pulses VAR Byte ' counter for motor control
temp VAR Byte
'
[ EEPROM Data ]
RunStatus DATA $00 ' run status
'
[ 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: ' set QTI black 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 ledspeaker, 100, 2500, 3000 ' beep each second
NEXT
GOTO Lunge ' start aggressive!
'
[ 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, [Search_For_Opponent, Spin_Left, Spin_Right, About_Face]
' --[ Border Avoidance ]--
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: ' left sensor was active
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
' --[ 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 forward
BRANCH irBits, [Scan, Follow_Right, Follow_Left, Lunge]
Scan:
BRANCH lastIR, [Move_Fwd, Scan_Right, Scan_Left]
Move_Fwd:
FOR pulses = 1 TO 40
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RFwdFast
NEXT
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, LRevSlow
PULSOUT RMotor, RFwdFast
lastIR = irBits
GOTO Main
Lunge: ' locked on -- go get him!
FOR pulses = 1 TO 40
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RFwdFast
NEXT
GOTO Main
' If SumoBot can see the opponent with both "eyes" and both QTIs are
' detecting the border, we must have pushed the opponent out.
'
[ 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, >=[500, 0], lbLeft ' 0 = black, 1 = line
LOOKDOWN rLine, >=[600, 0], lbRight
RETURN
Read_IR_Sensors:
FREQOUT LfIrOut, 1, 38000 ' modulate left IR LED
irLeft = ~LfIrIn ' read input (1 = target)
FREQOUT RtIrOut, 1, 38000 ' modulate right IR LED
irRight = ~RtIrIn ' read input (1 = target)
FREQOUT IrLedLS, 1, 38000 ' modulate lefts IR LED
irLSide = ~irLSide ' read input (1 = target)
FREQOUT IrLedRS, 1, 38000 ' modulate rights IR LED
irRSide = ~irRSide ' read input (1 = target)
RETURN
Creep_Forward:
FOR pulses = 1 TO 20
PULSOUT LMotor, LFwdSlow
PULSOUT RMotor, RFwdSlow
PAUSE 20
NEXT
RETURN
Comments
Read_IR_Sensors:
FREQOUT LfIrOut, 1, 38000 ' modulate left IR LED
irLeft = ~LfIrIn ' read input (1 = target)
FREQOUT RtIrOut, 1, 38000 ' modulate right IR LED
irRight = ~RtIrIn ' read input (1 = target)
FREQOUT IrLedLS, 1, 38000 ' modulate lefts IR LED
irLSide = ~irLSide ' read input (1 = target) don't you mean irLSide=~IrSenseLS
FREQOUT IrLedRS, 1, 38000 ' modulate rights IR LED
irRSide = ~irRSide ' read input (1 = target) don't you mean irRSide=~IrSenseRS
RETURN