SumoBot Basic Competition Code

EsponVEsponV Posts: 2
edited September 2012 in Robotics Vote Up0Vote Down
Greetings,

I am new to robotics and recently purchased the SumoBot kit. I've put the project together and it runs perfectly, but I have a couple questions about the code. I'll reference the last program in the book, the Basic Competition Program here to highlight my areas of confusion.
' 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


Speaker      PIN        1                       ' piezo speaker
StartLED     PIN        0                       ' display start delay


' -----[ 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


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 Speaker, 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 =                                   ' 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 = 
  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 = 
  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:
  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, LRevSlow
  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 = ) THEN Match_Over         ' in sight, we're on the line
  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.


Match_Over:
  FOR pulses = 1 TO 10                          ' stop motors
    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


' -----[ 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, >=[1000, 0], lbLeft           ' 0 = black, 1 = line
  LOOKDOWN rLine, >=[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

Within this code there is something called the blackThresh which according to the comment sets the threshold for the black reflectivity reading at 1/4 the average of the two sensor readings. However, blackThresh = (lLine / 10) + (rLine / 10). If it was really 1/4 the average shouldn't the variable be set as (lLine / 8) + (rLine / 8)?

Additionally, after this blackThresh is declared and set it is not called later in the code. Therefore, I don't understand why this is included in the program at all since it doesn't seem to have a purpose. Does anyone know why it is included / Am I missing something?

My last question is the order the lineBits and irBits get the left and right sensor data. lbLeft is set to the memory location of lineBits.BIT1 whereas lbRight is set to lineBits.BIT0. irBits shows the same pattern. When interpreting lineBits we see a branch: BRANCH lineBits, [Search_For_Opponent, Spin_Left, Spin_Right, About_Face]. I imagine this to be 00 = Search_For_Opponent; 01 = Spin_Left (since the right sensor hits the line); 10 = Spin_Right (since the left sensor hits the border); 11 = About_Face (since both have hit the border). If I have an array of length two I would address the positions as 0 and 1, where 0 would be on the left. Why then, is lbLeft getting lineBits.BIT1 and lbRight getting lineBits.BIT0? Does memory read right to left? Is there a better way for me to visualize this call? It seems backwards to me, but it works - hence my confusion.

Thank you all for the help. If you have any questions please let me know.

Comments

  • 2 Comments sorted by Date Added Votes
  • EsponVEsponV Posts: 2
    edited September 2012 Vote Up0Vote Down
    I found this in another thread on the forum (credit goes to Zoot in this thread: http://forums.parallax.com/showthread.php?90304-random-numbers-in-pbasic):
    Bits are numbered 0 - 15 in a Word and 0 - 7 in a Byte, etc., with 0 being the right-most or (L)east (S)ignificant (B)it and -- in this case -- 15 being the left-most or (M)ost (S)ignificant (B)it.

    So, with 0 being the right most bit this helps to explain why the right QTI sensor gets lineBits.BIT0 while the left sensor gets lineBits.BIT1. Is this correct?

    If anyone has insight on why blackThresh is included in the basic competition program I would appreciate you sharing it. I'm having quite a bit of fun with the robot.

    Thanks.
Sign In or Register to comment.