Scribbler Line Follower Program Help (intermediate/advanced BS2 programming)

the_professor

Hi all,

So I'm teaching a basic robotics class and I'm trying to reverse engineer the demo code that comes pre-loaded onto the Scribber. I'm trying to figure out exactly how some of the SELECT/CASE stuff is working, I just can't figure out what certain variables are for. If anybody has experience with intermediate/advanced programming, help a brother out? I can't for the life of me figure out what each case (0011 etc) actually corresponds to in terms of memory or variables.

Here's the relevant code:

' Scribbler robot demonstration code
' Copyright 2005, Element Products, Inc.
'
'· {$STAMP BS2}
'· {$PBASIC 2.5}
·
·
·
#DEFINE Verbose = 0
·
·
·
' I/O Declarations
·
LightRight··· PIN 0
LightCenter·· PIN 1
LightLeft···· PIN 2
LineEnable··· PIN 3
LineRight···· PIN 4
LineLeft····· PIN 5
ObsRx········ PIN 6
Stall········ PIN 7
LedRight····· PIN 8
LedCenter···· PIN 9
LedLeft······ PIN 10
Speaker······ PIN 11
MotorRight··· PIN 12
MotorLeft···· PIN 13
ObsTxRight··· PIN 14
ObsTxLeft···· PIN 15
·
·
·
' I/O Initialization
·
HIGH LightRight
HIGH LightCenter
HIGH LightLeft
LOW LineEnable
LOW LedRight
LOW LedCenter
LOW LedLeft
LOW Speaker
LOW MotorRight
LOW MotorLeft
LOW ObsTxRight
LOW ObsTxLeft
·
·
·
·
·
' Global Constants
·
VelStop CON 128
·
·
·
' Global Variables
·
light_l VAR Word
light_c VAR Word
light_r VAR Word
motor_r VAR Byte
motor_l VAR Byte
motor_temp VAR Byte
move_time VAR Byte
obs_r VAR Bit
obs_l VAR Bit
·
rand_w0 VAR Word
rand_t0 VAR rand_w0.BIT0
·
ram_w0 VAR Word
ram_w1 VAR Word
ram_w2 VAR Word
ram_b0 VAR Byte
ram_b1 VAR Byte
ram_b2 VAR Byte
ram_b3 VAR Byte
ram_n0 VAR Nib
ram_t0 VAR Bit
ram_t1 VAR Bit
·
·
·
·
·
' Check calibration and repair if necessary
DATA@0, (16)
·
ram_b0 = 0
·
FOR ram_b1 = 0 TO 3
· READ ram_b1, ram_b2
· ram_b0 = ram_b0 + ram_b2
NEXT
·
' Sum of all bytes including checksum (3) should be 0
' If not, write default values
IF (ram_b0) THEN
· FOR ram_b1 = 0 TO 3
··· WRITE ram_b1, 128:· ' = 1.0
· NEXT
ENDIF
·
' Line follow demo program
·
line_follow:
·
' Constants
·
LnfVelDrv CON 168
LnfVelTurnF CON 173
LnfVelTurnR CON 83
LnfVelArcF CON 178
LnfVelArcR CON 138
·
LnfMaxInit CON 30
LnfMaxInc CON 30
LnfMaxMax CON 120
·
·
' Variables
·
lnf_cnt VAR ram_b0
lnf_max VAR ram_b1
lnf_now VAR ram_n0
lnf_last VAR ram_t0
lnf_in VAR INS.NIB1
·
·
·
· FREQOUT Speaker, 200, 300, 1200
· FREQOUT Speaker, 500, 600, 1200
·
· HIGH LineEnable
· lnf_max = 30
· lnf_cnt = 30
·
·
line_follow_loop:
·
· IF (Stall = 1) THEN
··· motor_r = VelStop
··· motor_l = VelStop
··· GOSUB motor_set
··· FREQOUT Speaker, 1000, 600
line_follow_end:
··· GOTO line_follow_end
· ENDIF
·
· lnf_now = lnf_in
·
· SELECT (lnf_now & %0011)
··· CASE %0011
····· motor_r = LnfVelDrv
····· motor_l = LnfVelDrv
····· lnf_max = LnfMaxInit
····· lnf_cnt = LnfMaxInit
·
····· LOW LedRight
····· HIGH LedCenter
····· LOW LedLeft
·
··· CASE %0001
····· motor_r = LnfVelArcR
····· motor_l = LnfVelArcF
····· lnf_last = 1
····· lnf_max = LnfMaxInit
····· lnf_cnt = LnfMaxInit
·
····· HIGH LedRight
····· LOW LedCenter
····· LOW LedLeft
·
··· CASE %0010
····· motor_r = LnfVelArcF
····· motor_l = LnfVelArcR
····· lnf_last = 0
····· lnf_max = LnfMaxInit
····· lnf_cnt = LnfMaxInit
·
····· LOW LedRight
····· LOW LedCenter
····· HIGH LedLeft
·
··· CASE ELSE
····· lnf_cnt = lnf_cnt - 1
·
····· IF (lnf_cnt = 0) THEN
······· lnf_max = lnf_max + LnfMaxInc
······· IF (lnf_max > LnfMaxMax) THEN lnf_max = LnfMaxMax
······· lnf_cnt = lnf_max
·
······· IF (lnf_last = 1) THEN
········· lnf_last = 0
······· ELSE
········· lnf_last = 1
··· ····ENDIF
····· ENDIF
·
····· IF (lnf_last = 1) THEN
······· motor_r = LnfVelTurnR
······· motor_l = LnfVelTurnF
····· ELSE
······· motor_r = LnfVelTurnF
······· motor_l = LnfVelTurnR
····· ENDIF
·
· ENDSELECT
·
· move_time = 0
· GOSUB motor_set
· PAUSE 3
·
·
··GOTO line_follow_loop

' Motor command subroutine
'
' takes three parameters:
'·· motor_r = speed of the right motor (28 - 228)
'···· (28 = full reverse, 128 = stop, 228 = full forward)
'·· motor_l = speed of the left motor (28 - 228)
'···· (28 = full reverse, 128 = stop, 228 = full forward)
'·· move_time = duration of the motor command (0 - 200)
'···· (0 = indefinite, 1 = .025 seconds, 200 = 5 seconds)
'
' corrects the motor speed according to the correction factors
'·· stored in address 0 (right) and 1 (left) of the EEPROM.
'·· The correction factors are in the range 0 - 255.
'··· 64 = correction factor = 0.5
'·· 128 = correction factor = 1.0 (normal speed)
'·· 192 = correction factor = 1.5
·
motor_set:
· READ 0, motor_temp
· PULSOUT MotorRight, (((((((motor_r - 128) * motor_temp) >> 7) + 256) & 511) MIN 156) * 10) - 560
· PAUSE 1
· IF (move_time) THEN PULSOUT MotorRight, (move_time * 10) + 1000
·
· READ 1, motor_temp
· PULSOUT MotorLeft, (((((((motor_l - 128) * motor_temp) >> 7) + 256) & 511) MIN 156) * 10) - 560
· PAUSE 1
· IF (move_time) THEN PULSOUT MotorLeft, (move_time * 10) + 1000
·
· PAUSE move_time * 25
RETURN

·
·

MSDTech

The key is in the line:
lnf_in VAR INS.NIB1
This puts the contents of the fisrt 4 pins into a nib variable name that is transfered to a memory variable with the statement:
lnf_now = lnf_in
I'm not that familiar with the connections on the Scribbler, but I suspect that the first two input pins are the line following sensors, since the statement:
lnf_now & %0011 will set the two high order bits to zero.
Thus, the three conditions
0011 - on the line
0001 - to the left of the line
0010 - to the right of the line
(note the left and right terms above may be reversed depending if the line sense is a zero or a one.

the_professor

Thanks for your help, that makes a lot more sense now.