'{$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