Reading an encoder by polling
for  Basic Stamp 2

    The following circuit and software was presented by Lior Elazary to the Robotics Society of Southern California on July 14, 2001.  It interfaces a handmade encoder to the Basic Stamp 2 and reads the resulting signals using software polling techniques.  

It uses the QRB 1114 optical sensor  www.fairchildsemi.com/products/opto/datasheets/ir/QRB111X.pdf which can be bought from www.digikey.com as part number QRB1114 for $1.30.

Liorsencoder.jpg (59977 bytes)The picture to the right shows the encoder strips mounted to the wheel and the QRB1114 mounted to read the strips as they go by.

For more information on Lior's design, go to his website at www.liorelazary.com and check Robotics/Sparky


The first sample program just shows how to read the encoders:

''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
''' READING_ENCODERS.BS2 - Program to read the encoder
''' AUTHORS: Lior Elazary (lelazary@yahoo.com)
''' LOG: 7/3/01 Created (Lior Elazary)
''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
' Wheels Vars

Right_Wheel         Con 0     ' The right wheel pin connection
Left_Wheel           Con 1     ' The left wheel pin connection

Right_Encoder Var     IN2     ' The pin the right encoder is attached
Left_Encoder Var       IN3     ' The pin the left encoder is attached

Right_Stop     Con 650     ' The right stop value determined by simple.bs2
Left_Stop       Con 642

Right_Speed     Var Word
Left_Speed       Var Word

Right_Encode_Speed     Var byte
Left_Encode_Speed       Var word

Last_Right_Encoder       Var Bit
Last_Left_Encoder         Var Bit

Odom         Var word


X     Var byte
T     Var byte


Input 2
Input 3

Speed     Con 30

Move_Forward:
Right_Speed = Right_Stop - Speed         'Set the right and left speed
Left_Speed = Left_Stop + Speed
Odom = 0 ' Reset odometer

Move_Forward_Loop:
    Right_Encode_Speed = 0 'reset encode speed values
    Left_Encode_Speed = 0
    For x = 1 to 46 'loop 46 times to make about 1s
        pulsout Right_Wheel, Right_Speed 'set the servos speed
        Pulsout Left_Wheel, Left_Speed
        For t = 1 to 10 ' Loop 10 times to make ~21ms of loop 
            If Last_Right_Encoder = Right_Encoder THEN Cont_Right 'count changes from 1 to 0 and 0 to 1
                Last_Right_Encoder = Right_Encoder
                Right_Encode_Speed = Right_Encode_Speed + 1
            Cont_Right:
            If Last_Left_Encoder = Left_Encoder THEN Cont_Left
                Last_Left_Encoder = Left_Encoder
                Left_Encode_Speed = Left_Encode_Speed + 1
            Cont_Left:

        Next
Next                                                  ' ~1 sec total time for loop (21ms+1ms) * 46 = 1.012s 
pulsout Right_Wheel, Right_Speed     ' Set the servos speed again to give about 21ms for calculations
Pulsout Left_Wheel, Left_Speed

Odom = Odom + ((Right_Encode_Speed + Left_Encode_Speed) / 2)     ' Set the odometer to the average values of the two speeds

'''
''' Speed is at encode_speed/32 RPS
'''
debug "ODOM:", dec Odom, " RS:", dec Right_Encode_Speed, " ", dec (Right_Encode_Speed/32), " LS:", dec Left_Encode_Speed, CR 'Display results

Goto Move_Forward_Loop


The second program uses the encoders to perform a task which was demonstrated by Lior at the meeting.

''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
''' SIMPLE_NAV.BS2 - Program to drive the robot about 11ft rotate 180deg and come back to the starting point
''' AUTHORS: Lior Elazary (lelazary@yahoo.com)
''' LOG: 7/3/01 Created (Lior Elazary)
''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''

' Wheels Vars
Right_Wheel        Con 0     ' The right and left wheel pin connection
Left_Wheel          Con 1 

Right_Encoder     Var     IN2    ' The pins the right and left encoder is attached
Left_Encoder       Var     IN3 

Right_Stop     Con      650      ' The right and left stop value determined by simple.bs2
Left_Stop      Con       642

Right_Speed     Var    Word      ' Right and left speed
Left_Speed       Var    Word

Right_Encode_Speed    Var    byte    ' The actual right and left encoded speed
Left_Encode_Speed      Var    byte

Last_Right_Encoder    Var   Bit    ' The bit to see if the sensor has tripped
Last_Left_Encoder      Var   Bit

Right_Od    Var    Word     ' Right and left odometer values
Left_Od      Var    Word

'Pid Vars
Error                Var    Word    ' The error for the pid
Right_Integ       Var    Word    ' The right and left integral
Left_Integ         Var    Word
Integ                 Var    Word    ' The current integral (used to save var space)
Prop                 Var    Word    ' The current proportion (used to save var space)


KRight_Prop     Con     $00F0     ' Constants for the right and left proportional, integral, and feed forward
KRight_Integ     Con     $0000
KRight_FF        Con     $0150

KLeft_Prop       Con     $00B0
KLeft_Integ       Con     $0000
KLeft_FF          Con     $0130


X        Var     byte
T        Var      byte
Sign    var       bit        ' the sign to do fractions 

Distance Var Word ' distance to travel

Input 2
Input 3

DSpeed    Con     30     ' The desired speed the wheels should be turning at


pause 6000    ' pause for 6sec used to align the robot

Right_Od = 0     ' reset odometer values
Left_Od = 0

''' Move Forward
Distance = 430            '~ 11 ft ~0.3068 inches per tic X 430 = 131.924 inches = 10.99 ft
gosub Move_Forward

Distance = 600            ' The distance now becomes the number of tics to rotate the wheels
''' Rotate 180 deg
Gosub Rotate_Right

''' Move Forward
Distance = 430             '~ 11 ft
gosub Move_Forward


End


''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
''' Move the robot forward Distance amount while keeping the wheels volsitis the same '''
''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''

Move_Forward:
Right_Speed = Right_Stop - (KRight_FF */ DSpeed)      ' Set the initial speed of the servos to get the motors up to                                                                                                 speed (Feed forward)
Left_Speed = Left_Stop + (KLeft_FF */ DSpeed)
Right_Od = 0                                                                   ' Reset odometors
Left_Od = 0

Move_Forward_Loop:
Right_Encode_Speed = 0                                                 ' Reset encoded speed
Left_Encode_Speed = 0
For x = 1 to 46                                                                 ' Loop 46 times to make about 1s
    pulsout Right_Wheel, Right_Speed                                 ' Set the right and left motor speed 
    Pulsout Left_Wheel, Left_Speed
    For t = 1 to 10 ' Loop 10 times to make ~21ms of loop
        If Last_Right_Encoder = Right_Encoder THEN Cont_Right ' Count changes from 1 to 0 and 0 to 1
            Last_Right_Encoder = Right_Encoder
            Right_Encode_Speed = Right_Encode_Speed + 1
        Cont_Right:
        If Last_Left_Encoder = Left_Encoder THEN Cont_Left
            Last_Left_Encoder = Left_Encoder
            Left_Encode_Speed = Left_Encode_Speed + 1
        Cont_Left:

    Next
Next                                                                         ' ~1 sec total time for loop (21ms+1ms) * 46 = 1.012s

Right_Od = Right_Od + Right_Encode_Speed         ' Updated left and right odometer
Left_Od = Left_Od + Left_Encode_Speed

if ( ((Right_Od + Left_Od) /2) > Distance) Then Stop_Robot     ' Check the odometer if the desired distance has                                                                                                         been reached

pulsout Right_Wheel, Right_Speed                                             ' Set the servos speed again to give about 21ms for calculations
Pulsout Left_Wheel, Left_Speed


'''''''''''''''''''''''''''''''''
'''' PID Routines '''
''''''''''''''''''''''''''''''''' 

Error = DSpeed-Right_Encode_Speed 'Calc Error

''' Calc Proportional

Sign = Error.bit15 ^ 0                                 ' Get the error sign
Prop = abs KRight_Prop */ abs Error         ' Multiply and return the middle 16 bits for fractions
if Sign = 0 then Skip_Right_Prop_Sign         ' Set the sign if set
Prop = - Prop
Skip_Right_Prop_Sign 

''' Calc Integral 
Right_Integ = Right_Integ + Error
Sign = Right_Integ.bit15 ^ 0

Integ = abs KRight_Integ */ abs Right_Integ
if Sign = 0 then skip_Right_Integ_sign
Integ = -Integ
skip_Right_Integ_sign:


Right_Speed = Right_Stop - ((KRight_FF */ DSpeed) + Integ + Prop)     ' Set the right Speed

' Calc Left Speed

Error = DSpeed-Left_Encode_Speed                                                         'Calc Error

' Calc Proportional
Sign = Error.bit15 ^ 0
Prop = abs KLeft_Prop */ abs Error
if Sign = 0 then Skip_Left_Prop_Sign
Prop = - Prop
Skip_Left_Prop_Sign 

' Calc Integral 
Left_Integ = Left_Integ + Error
Sign = Left_Integ.bit15 ^ 0

Integ = abs KLeft_Integ */ abs Left_Integ
if Sign = 0 then skip_Left_Integ_sign
Integ = -Integ
skip_Left_Integ_sign:

Left_Speed = Left_Stop + ((KLeft_FF */ DSpeed) + Integ + Prop)         ' Set Left Speed



debug dec Right_Encode_Speed, " ", dec Left_Encode_Speed, CR

Goto Move_Forward_Loop


''''''''''''''''''''''''''''''''''''''''''''
''' Rotate the robot right Distance tics '''
''''''''''''''''''''''''''''''''''''''''''''
Rotate_Right: 
Right_Od = 0
Left_Od = 0
Right_Speed = Right_Stop + (KRight_FF */ DSpeed)
Left_Speed = Left_Stop + (KLeft_FF */ DSpeed)

Right_Encode_Speed = 0
Left_ENcode_Speed = 0
For x = 1 to 100
pulsout Right_Wheel, Right_Speed
Pulsout Left_Wheel, Left_Speed
For t = 1 to 10 ' To make ~21ms of loop
If Last_Right_Encoder = Right_Encoder THEN Cont_Right_Rotate
Last_Right_Encoder = Right_Encoder
Right_Encode_Speed = Right_Encode_Speed + 1
Cont_Right_Rotate:
If Last_Left_Encoder = Left_Encoder THEN Cont_Left_Rotate
Last_Left_Encoder = Left_Encoder
Left_Encode_Speed = Left_Encode_Speed + 1
Cont_Left_Rotate:

Next
Right_Od = Right_Od + Right_Encode_Speed
Left_Od = Left_Od + Left_Encode_Speed
if ( ((Right_Od + Left_Od) /2) > Distance) Then Stop_Robot    ' Stop the robot if odometer has reached the Distance

Next                                                                                         ' ~2.1 sec total of loop

debug dec Left_Od, " ", dec Right_Od, CR

goto Stop_Robot


''''''''''''''''''''''''''''''''''''''''
''' Stop the Robot '''
''''''''''''''''''''''''''''''''''''''''

Stop_Robot: 
Right_Speed = Right_Stop
Left_Speed = Left_Stop
Pulsout Right_Wheel, Right_Speed
Pulsout Left_Wheel, Left_Speed

Return