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.
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