Shop OBEX P1 Docs P2 Docs Learn Events
Sumobot Code: side IR's not working — Parallax Forums

Sumobot Code: side IR's not working

joy1joy1 Posts: 32
edited 2011-03-22 06:59 in Robotics
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

Comments

  • ercoerco Posts: 20,256
    edited 2011-03-22 00:23
    Check this subroutine:

    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
  • joy1joy1 Posts: 32
    edited 2011-03-22 06:59
    I will try it out and let you know ...thanks
Sign In or Register to comment.