EH2 Robot - SD21 Servo Driver Module
Demonstration Software Example for use with Parallax Basic Stamp BS2p

The following shows how to use the SD21 Servo Controller and SRF08 ultrasonic ranger with the Lynxmotion EH2 robot. This is the software used in the EH2 Video.

Left - EH2 Robot after assembly

Right - Close up of the SD21 module

Connections
This example uses a single 7.2v battery to power the logic and the servo's, so a link must be fitted to the 2-pin header near the screw terminals. The servo's must be connected as indicated and a Parallax Basic Stamp BS2p fitted to the 24 pin socket. One of the I2C connectors is used to connect to an SRF08 ultrasonic ranger.

  

 

 Servo 21 - unused
Servo 20 - unused
Servo 19 - unused
Servo 18 - unused
Servo 17 - unused
Servo 16 - unused
Servo 15 - unused
Servo 14 - Right Rear Lift
Servo 13 - Right Rear Propulsion
Servo 12 - Left Rear Lift
Servo 11 - Left Rear Propulsion
Servo 10 - Right Middle Lift
Servo 9 - Right Middle Propulsion
Servo 8 - Left Middle Lift
Servo 7 - Left Middle Propulsion
Servo 6 - Right Front Lift
Servo 5 - Right Front Propulsion
Servo 4 - Left Front Lift
Servo 3 - Left Front Propulsion
Servo 2 - Sonar Up/Down
Servo 1 - Sonar Left/Right
Behavior
This software will move the robot around by walking forwards. The head tilt is fixed in a proud position and pan moves it from side to side as the SRF08 ranges left and right. If an object is encountered that is less than 24 inches away, the robot turn left or right as appropriate. The code contains routines for standing which is useful when aligning the servo's, also for sitting, and moving backwards. The later two not being used in this example but are provided for you to experiment with. Have fun!

The following software can be downloaded here.
 
'{$STAMP BS2p}

'***********************************************************
'**                                                       **
'**  I2C Routines for the Basic Stamp BS2p RevC or later  **
'**        with the SD21 Servo Control Modules            **
'**           and the Lynxmotion EH2 Robot                **
'**                                                       **
'**            Copyright 2004 - Devantech Ltd             **
'**     Commercial use of this software is prohibited     **
'**     Private and educational use only is permitted     **
'**                                                       **
'**            Written by Gerald Coe - July 2004          **
'**                                                       **
'***********************************************************

' To use this software with the EH2 the servo's must be connected as shown on line below:
' Servo 1 - Sonar Left/Right
' Servo 2 - Sonar Up/Down
' Servo 3 - Left Front Propulsion
' Servo 4 - Left Front Lift
' Servo 5 - Right Front Propulsion
' Servo 6 - Right Front Lift
' Servo 7 - Left Middle Propulsion
' Servo 8 - Left Middle Lift
' Servo 9 - Right Middle Propulsion
' Servo 10 - Right Middle Lift
' Servo 11 - Left Rear Propulsion
' Servo 12 - Left Rear Lift
' Servo 13 - Right Rear Propulsion
' Servo 14 - Right Rear Lift

' This example shows how to control the SD21/EH2 robot using 16-bit position information
' Writing to a servo's is done using a single I2COUT instruction which writes
' the speed, position low byte and postion high byte in one transaction
' Naming conventions used are as follows:
' LFlift the register address of the servo that raises or lowers the Left Front leg
' oLFlift the nominal centre position of the Left Front leg
' hLFlift high byte of the nominal centre position of the Left Front leg
' uLFlift the upper position of the Left Front leg
' dLFlift the lower position of the Left Front leg
' huLFlift the high byte of the upper position of the Left Front leg
' hdLFlift the high byte of the lower position of the Left Front leg

' Position constants are calculated by the compiler from a few simpler constants, such
' as the lift, stride and centre positions which are easily changed for experimenting

' Define the centre positions of the servo's, I prefer to define the centres here rather
' than adjusting the servo screws. However, if you need to go too far from this, then the
' servo screws should be adjusted. 1500 is the servo's nominal centre postion.
oUDlook CON 1700 ' Up/down position on pan/tilt head
oLRlook CON 1480 ' Left/right position on pan/tilt head
oLFstride CON 1440 ' Left Front Propulsion
oLFlift CON 1200 ' Left Front Lift
oRFstride CON 1560 ' Right Front
oRFlift CON 1500
oLMstride CON 1520 ' Left Middle
oLMlift CON 1650
oRMstride CON 1500 ' Right Middle
oRMlift CON 1500
oLRstride CON 1570 ' Left Rear
oLRlift CON 1500
oRRstride CON 1450 ' Right Rear
oRRlift CON 1400

Spd CON 30 ' 0 max, 1 slowest, 255 fastest speed
Sdly CON 200 ' Stride delay in mS
Ldly CON 65 ' Lift delay in mS
Lift CON 300 ' Lift distance either side of centre
Stride CON 220 ' Stride distance either side of centre
Look CON 200 ' Look distance either side of centre

Ldown CON 1000 ' Left & right leg heights for sitting
Rdown CON 2000
hLdown CON 1000/256
hRdown CON 2000/256

' The constants above are the only ones that need be changed for various lift, stride and
' speeds of your robot. The constants below are calculated by the compiler from the
' simpler primitives above.

lLook CON oLRlook-Look ' Define Left & Right look postions
rLook CON oLRlook+Look
hlLook CON lLook/256
hrLook CON rLook/256
hLRlook CON oLRlook/256
hUDlook CON oUDlook/256

hLFstride CON oLFstride/256 ' Left Front Propulsion
hLFlift CON oLFlift/256 ' Left Front Lift
hRFstride CON oRFstride/256 ' Right Front
hRFlift CON oRFlift/256
hLMstride CON oLMstride/256 ' Left Middle
hLMlift CON oLMlift/256
hRMstride CON oRMstride/256 ' Right Middle
hRMlift CON oRMlift/256
hLRstride CON oLRstride/256 ' Left Rear
hLRlift CON oLRlift/256
hRRstride CON oRRstride/256 ' Right Rear
hRRlift CON oRRlift/256

fLFstride CON oLFstride-stride ' Left Front
bLFstride CON oLFstride+stride
uLFlift CON oLFlift-lift
dLFlift CON oLFlift+lift
fRFstride CON oRFstride+stride ' Right Front
bRFstride CON oRFstride-stride
uRFlift CON oRFlift+lift
dRFlift CON oRFlift-lift
fLMstride CON oLMstride-stride ' Left Middle
bLMstride CON oLMstride+stride
uLMlift CON oLMlift-lift
dLMlift CON oLMlift+lift
fRMstride CON oRMstride+stride ' Right Middle
bRMstride CON oRMstride-stride
uRMlift CON oRMlift+lift
dRMlift CON oRMlift-lift
fLRstride CON oLRstride-stride ' Left Rear
bLRstride CON oLRstride+stride
uLRlift CON oLRlift-lift
dLRlift CON oLRlift+lift
fRRstride CON oRRstride+stride ' Right Rear
bRRstride CON oRRstride-stride
uRRlift CON oRRlift+lift
dRRlift CON oRRlift-lift

hfLFstride CON fLFstride/256 ' Left Front
hbLFstride CON bLFstride/256
huLFlift CON uLFlift/256
hdLFlift CON dLFlift/256
hfRFstride CON fRFstride/256 ' Right Front
hbRFstride CON bRFstride/256
huRFlift CON uRFlift/256
hdRFlift CON dRFlift/256
hfLMstride CON fLMstride/256 ' Left Middle
hbLMstride CON bLMstride/256
huLMlift CON uLMlift/256
hdLMlift CON dLMlift/256
hfRMstride CON fRMstride/256 ' Right Middle
hbRMstride CON bRMstride/256
huRMlift CON uRMlift/256
hdRMlift CON dRMlift/256
hfLRstride CON fLRstride/256 ' Left Rear
hbLRstride CON bLRstride/256
huLRlift CON uLRlift/256
hdLRlift CON dLRlift/256
hfRRstride CON fRRstride/256 ' Right Rear
hbRRstride CON bRRstride/256
huRRlift CON uRRlift/256
hdRRlift CON dRRlift/256

' define the register positions for the servo's used
' Servo's use a group of three register, Speed, Pos.lowbyte, Pos.highbyte
' Set speed to $00 for maximum speed, $01 for slowest speed
' Pos is set in uS
LRlook CON 0 ' Servo 1 - sonar Left/Right
UDlook CON 3 ' Servo 2 - Up/Down
LFstride CON 6 ' Servo 3 - Left Front Propulsion
LFlift CON 9 ' Servo 4 - Left Front Lift
RFstride CON 12 ' Servo 5 - Right Front
RFlift CON 15 ' Servo 6
LMstride CON 18 ' Servo 7 - Left Middle
LMlift CON 21 ' Servo 8
RMstride CON 24 ' Servo 9 - Right Middle
RMlift CON 27 ' Servo 10
LRstride CON 30 ' Servo 11 - Left Rear
LRlift CON 33 ' Servo 12
RRstride CON 36 ' Servo 13 - Right Rear
RRlift CON 39 ' Servo 14

SDA CON 0 ' SDA on pin0, SCL on pin1

lightL VAR Byte ' light sensor data is collected but
lightR VAR Byte ' unused in this demo
rangeL VAR Word ' Left range in inches
rangeR VAR Word ' Right range in inches
LeftLook VAR Bit ' indicates direction currently looking

Start:
PAUSE 200
I2COUT SDA, $c2, UDlook, [0, oUDlook, hUDlook]
I2COUT SDA, $c2, UDlook, [0, oUDlook, hUDlook]

Setup:
' GOSUB Stand ' uncomment these two lines only
' GOTO Setup ' when setting up servo positions

Test:
' GOSUB Fwd ' uncomment these two lines for
' GOTO Test ' uncontrolled forward walking

' This simple main loop sets the robot walking forward unless there
' is an object left or right that needs avoiding
Loop:
I2COUT SDA, $c2, UDlook, [0, oUDlook, hUDlook]
IF rangeL<24 THEN Object_left
IF rangeR<24 THEN Object_right
GOSUB Fwd ' Nothing about, so onwards
GOTO Loop
Object_left: ' Object on left, so turn right
GOSUB Right
GOTO Loop
Object_right: ' Object on Right, so turn left
GOSUB Left
GOTO Loop


' Walk forward one cycle
Fwd:
I2COUT SDA, $c2, LFlift, [0, uLFlift, huLFlift]
I2COUT SDA, $c2, RMlift, [0, uRMlift, huRMlift]
I2COUT SDA, $c2, LRlift, [0, uLRlift, huLRlift]
I2COUT SDA, $c2, RFstride, [Spd, bRFstride, hbRFstride]
I2COUT SDA, $c2, LMstride, [Spd, bLMstride, hbLMstride]
I2COUT SDA, $c2, RRstride, [Spd, bRRstride, hbRRstride]
GOSUB sonar_ping
PAUSE Ldly
GOSUB sonar_read
I2COUT SDA, $c2, LFstride, [0, fLFstride, hfLFstride]
I2COUT SDA, $c2, RMstride, [0, fRMstride, hfRMstride]
I2COUT SDA, $c2, LRstride, [0, fLRstride, hfLRstride]
PAUSE Sdly
I2COUT SDA, $c2, LFlift, [0, dLFlift, hdLFlift]
I2COUT SDA, $c2, RMlift, [0, dRMlift, hdRMlift]
I2COUT SDA, $c2, LRlift, [0, dLRlift, hdLRlift]
PAUSE Ldly
I2COUT SDA, $c2, RFlift, [0, uRFlift, huRFlift]
I2COUT SDA, $c2, LMlift, [0, uLMlift, huLMlift]
I2COUT SDA, $c2, RRlift, [0, uRRlift, huRRlift]
I2COUT SDA, $c2, LFstride, [Spd, bLFstride, hbLFstride]
I2COUT SDA, $c2, RMstride, [Spd, bRMstride, hbRMstride]
I2COUT SDA, $c2, LRstride, [Spd, bLRstride, hbLRstride]
GOSUB sonar_ping
PAUSE Ldly
GOSUB sonar_read
I2COUT SDA, $c2, RFstride, [0, fRFstride, hfRFstride]
I2COUT SDA, $c2, LMstride, [0, fLMstride, hfLMstride]
I2COUT SDA, $c2, RRstride, [0, fRRstride, hfRRstride]
PAUSE Sdly
I2COUT SDA, $c2, RFlift, [0, dRFlift, hdRFlift]
I2COUT SDA, $c2, LMlift, [0, dLMlift, hdLMlift]
I2COUT SDA, $c2, RRlift, [0, dRRlift, hdRRlift]
PAUSE Ldly
RETURN

' Walk backward one cycle
Back:
I2COUT SDA, $c2, RFlift, [0, uRFlift, huRFlift]
I2COUT SDA, $c2, LMlift, [0, uLMlift, huLMlift]
I2COUT SDA, $c2, RRlift, [0, uRRlift, huRRlift]
I2COUT SDA, $c2, LFstride, [Spd, fLFstride, hfLFstride]
I2COUT SDA, $c2, RMstride, [Spd, fRMstride, hfRMstride]
I2COUT SDA, $c2, LRstride, [Spd, fLRstride, hfLRstride]
GOSUB sonar_ping
PAUSE Ldly
GOSUB sonar_read
I2COUT SDA, $c2, RFstride, [0, bRFstride, hbRFstride]
I2COUT SDA, $c2, LMstride, [0, bLMstride, hbLMstride]
I2COUT SDA, $c2, RRstride, [0, bRRstride, hbRRstride]
PAUSE Sdly
I2COUT SDA, $c2, RFlift, [0, dRFlift, hdRFlift]
I2COUT SDA, $c2, LMlift, [0, dLMlift, hdLMlift]
I2COUT SDA, $c2, RRlift, [0, dRRlift, hdRRlift]
PAUSE Ldly
I2COUT SDA, $c2, LFlift, [0, uLFlift, huLFlift]
I2COUT SDA, $c2, RMlift, [0, uRMlift, huRMlift]
I2COUT SDA, $c2, LRlift, [0, uLRlift, huLRlift]
I2COUT SDA, $c2, RFstride, [Spd, fRFstride, hfRFstride]
I2COUT SDA, $c2, LMstride, [Spd, fLMstride, hfLMstride]
I2COUT SDA, $c2, RRstride, [Spd, fRRstride, hfRRstride]
GOSUB sonar_ping
PAUSE Ldly
GOSUB sonar_read
I2COUT SDA, $c2, LFstride, [0, bLFstride, hbLFstride]
I2COUT SDA, $c2, RMstride, [0, bRMstride, hbRMstride]
I2COUT SDA, $c2, LRstride, [0, bLRstride, hbLRstride]
PAUSE Sdly
I2COUT SDA, $c2, LFlift, [0, dLFlift, hdLFlift]
I2COUT SDA, $c2, RMlift, [0, dRMlift, hdRMlift]
I2COUT SDA, $c2, LRlift, [0, dLRlift, hdLRlift]
PAUSE Ldly
RETURN

' Step right one cycle
Right:
I2COUT SDA, $c2, RFlift, [0, uRFlift, huRFlift]
I2COUT SDA, $c2, RRlift, [0, uRRlift, huRRlift]
I2COUT SDA, $c2, RMstride, [Spd, fRMstride, hfRMstride]
I2COUT SDA, $c2, LFlift, [0, uLFlift, huLFlift]
I2COUT SDA, $c2, LRlift, [0, uLRlift, huLRlift]
I2COUT SDA, $c2, LMstride, [Spd, bLMstride, hbLMstride]
GOSUB sonar_ping
PAUSE Ldly
GOSUB sonar_read
I2COUT SDA, $c2, LFstride, [0, fLFstride, hfLFstride]
I2COUT SDA, $c2, LRstride, [0, fLRstride, hfLRstride]
I2COUT SDA, $c2, RFstride, [0, bRFstride, hbRFstride]
I2COUT SDA, $c2, RRstride, [0, bRRstride, hbRRstride]
PAUSE Sdly
I2COUT SDA, $c2, LFlift, [0, dLFlift, hdLFlift]
I2COUT SDA, $c2, LRlift, [0, dLRlift, hdLRlift]
I2COUT SDA, $c2, RFlift, [0, dRFlift, hdRFlift]
I2COUT SDA, $c2, RRlift, [0, dRRlift, hdRRlift]
PAUSE Ldly
I2COUT SDA, $c2, LMlift, [0, uLMlift, huLMlift]
I2COUT SDA, $c2, RMlift, [0, uRMlift, huRMlift]
I2COUT SDA, $c2, LFstride, [Spd, bLFstride, hbLFstride]
I2COUT SDA, $c2, LRstride, [Spd, bLRstride, hbLRstride]
I2COUT SDA, $c2, RFstride, [Spd, fRFstride, hfRFstride]
I2COUT SDA, $c2, RRstride, [Spd, fRRstride, hfRRstride]
GOSUB sonar_ping
PAUSE Ldly
GOSUB sonar_read
I2COUT SDA, $c2, LMstride, [0, fLMstride, hfLMstride]
I2COUT SDA, $c2, RMstride, [0, bRMstride, hbRMstride]
PAUSE Sdly
I2COUT SDA, $c2, LMlift, [0, dLMlift, hdLMlift]
I2COUT SDA, $c2, RMlift, [0, dRMlift, hdRMlift]
PAUSE Ldly
RETURN

' Step left one cycle
Left:
I2COUT SDA, $c2, LMlift, [0, uLMlift, huLMlift]
I2COUT SDA, $c2, LFstride, [Spd, fLFstride, hfLFstride]
I2COUT SDA, $c2, LRstride, [Spd, fLRstride, hfLRstride]
I2COUT SDA, $c2, RMlift, [0, uRMlift, huRMlift]
I2COUT SDA, $c2, RFstride, [Spd, bRFstride, hbRFstride]
I2COUT SDA, $c2, RRstride, [Spd, bRRstride, hbRRstride]
GOSUB sonar_ping
PAUSE Ldly
GOSUB sonar_read
I2COUT SDA, $c2, LMstride, [0, bLMstride, hbLMstride]
I2COUT SDA, $c2, RMstride, [0, fRMstride, hfRMstride]
PAUSE Sdly
I2COUT SDA, $c2, LMlift, [0, dLMlift, hdLMlift]
I2COUT SDA, $c2, RMlift, [0, dRMlift, hdRMlift]
PAUSE Ldly
I2COUT SDA, $c2, RFlift, [0, uRFlift, huRFlift]
I2COUT SDA, $c2, RRlift, [0, uRRlift, huRRlift]
I2COUT SDA, $c2, RMstride, [Spd, bRMstride, hbRMstride]
I2COUT SDA, $c2, LFlift, [0, uLFlift, huLFlift]
I2COUT SDA, $c2, LRlift, [0, uLRlift, huLRlift]
I2COUT SDA, $c2, LMstride, [Spd, fLMstride, hfLMstride]
GOSUB sonar_ping
PAUSE Ldly
GOSUB sonar_read
I2COUT SDA, $c2, LFstride, [0, bLFstride, hbLFstride]
I2COUT SDA, $c2, LRstride, [0, bLRstride, hbLRstride]
I2COUT SDA, $c2, RFstride, [0, fRFstride, hfRFstride]
I2COUT SDA, $c2, RRstride, [0, fRRstride, hfRRstride]
PAUSE Sdly
I2COUT SDA, $c2, LFlift, [0, dLFlift, hdLFlift]
I2COUT SDA, $c2, LRlift, [0, dLRlift, hdLRlift]
I2COUT SDA, $c2, RFlift, [0, dRFlift, hdRFlift]
I2COUT SDA, $c2, RRlift, [0, dRRlift, hdRRlift]
PAUSE Ldly
RETURN

' Moves legs to a centre standing position
Stand:
I2COUT SDA, $c2, LFlift, [0, uLFlift, huLFlift]
I2COUT SDA, $c2, RMlift, [0, uRMlift, huRMlift]
I2COUT SDA, $c2, LRlift, [0, uLRlift, huLRlift]
PAUSE Ldly
I2COUT SDA, $c2, RMstride, [0, oRMstride, hRMstride, 0, oRMlift, hRMlift]
I2COUT SDA, $c2, LFstride, [0, oLFstride, hLFstride, 0, oLFlift, hLFlift]
I2COUT SDA, $c2, LRstride, [0, oLRstride, hLRstride, 0, oLRlift, hLRlift]
PAUSE Ldly
I2COUT SDA, $c2, LFlift, [0, dLFlift, hdLFlift]
I2COUT SDA, $c2, RMlift, [0, dRMlift, hdRMlift]
I2COUT SDA, $c2, LRlift, [0, dLRlift, hdLRlift]
PAUSE Ldly
I2COUT SDA, $c2, RFlift, [0, uRFlift, huRFlift]
I2COUT SDA, $c2, LMlift, [0, uLMlift, huLMlift]
I2COUT SDA, $c2, RRlift, [0, uRRlift, huRRlift]
PAUSE Ldly
I2COUT SDA, $c2, RFstride, [0, oRFstride, hRFstride, 0, oRFlift, hRFlift]
I2COUT SDA, $c2, LMstride, [0, oLMstride, hLMstride, 0, oLMlift, hLMlift]
I2COUT SDA, $c2, RRstride, [0, oRRstride, hRRstride, 0, oRRlift, hRRlift]
PAUSE Ldly
RETURN

' Makes the robot sit down
Sit:
GOSUB Stand
I2COUT SDA, $c2, LFlift, [0, Ldown, hLdown]
I2COUT SDA, $c2, LRlift, [0, Ldown, hLdown]
I2COUT SDA, $c2, LMlift, [0, Ldown, hLdown]
I2COUT SDA, $c2, RFlift, [0, Rdown, hRdown]
I2COUT SDA, $c2, RMlift, [0, Rdown, hRdown]
I2COUT SDA, $c2, RRlift, [0, Rdown, hRdown]
PAUSE Ldly

sonar_ping:
I2COUT SDA, $e0, 0, [80] ' Rangeing command - 80 for inches, 81 for cm, 82 for uS
RETURN

' Reads the Left or Right sonar position, then moves
' head to next position
sonar_read:
IF LeftLook=0 THEN getright
LeftLook = 0
I2CIN SDA, $e1, 1, [lightL, rangeL.HIGHBYTE, rangeL.LOWBYTE]
I2COUT SDA, $c2, LRlook, [0, rLook, hrLook]
GOTO s1
getright:
LeftLook = 1
I2CIN SDA, $e1, 1, [lightR, rangeR.HIGHBYTE, rangeR.LOWBYTE]
I2COUT SDA, $c2, LRlook, [0, lLook, hlLook]
s1:
RETURN