' www.crustcrawler.com ' Copyright 2006 CrustCrawler Inc. ' '------- [ HexCrawlerBase.BS2 ] ---------------------------------------------- '{$STAMP BS2} '{$PBASIC 2.5} ' ' File....... HexCrawlerBase.BS2 ' Purpose.... Investigate program parameters. ' Create and test custom walking gaits. ' Author..... CrustCrawler Inc. (Mike Gebhard) ' E-mail..... support@crustcrawler.com ' Started.... 11/03/2005 ' Updated.... 04/9/2006 ' Version.... 2.0 ' ' Hardware ' (1) HexCrawler Robotic Kit ' (1) Parallax BOE ' (1) Parallax Basic Stamp II ' (1) Parallax PSC ' '========================================================================= ' Updates 3/11/2006 '========================================================================= ' Added the following subroutines ' * Limit_Stride ' * Set_Ramp_Delay ' ' Limit_Stride reduces horizontal swing (stride) by the amount stored in ' the limiter variable. ' ' Set_Ramp_Delay takes the limiter value and calculates ramp and delay ' values. ' ' REASON FOR THE CHANGES: ' The prior base code allowed for an adjustable gait values but once ' downloaded and placed into the EEPROM the values became static. ' In order to change gait values, the source code needed to be changed ' and downloaded again. You can now change several gait variables ' during run time. There is a cost, more code and variable space is ' used. ' ' Spinnig in place can be tough on the center legs. Reducing the stride ' while spinning lowers the stress on the center lags. ' ' Lastly, the code is more robust. ' '========================================================================= ' Updates 1/17/2006 '========================================================================= ' * Added LSpin and RSpin EEPROM tables. These new tables improve spin ' capabilities. ' * Commented out old LTurn and RTurn EEPROM tables and moved them to the ' bottom of this file. ' * Added Neutral EEPROM table. Use this table to adjust legs if needed. ' '========================================================================= ' Overview '========================================================================= ' HexCrawlerBase.BS2 allows you to FULLY control ' each leg joint. ' * Control leg joint speed, rotation, and sequence programmatically. ' * Modify default gait speed and stride for custom walking. ' * Create and test custom gaits by adding new EEPROM blocks. ' * This code accepts all the Parallax Stamp modules. ' '========================================================================= ' Getting Started '========================================================================= ' 1. Change the default leg centers in this program with the legs ' centers found in TuneHex.BS2. ' http://www.crustcrawler.com/products/HexCrawler.php?id=19 ' ' Find the section of code below '-----[ Horizontal Leg Constants ]---------------------------------------- ' Center1 CON 750 ' Leg1 horizontal servo ' Center2 CON 750 ' Leg2 horizontal servo ' Center3 CON 750 ' Leg3 horizontal servo ' Center4 CON 750 ' Leg4 horizontal servo ' Center5 CON 750 ' Leg4 horizontal servo ' Center6 CON 750 ' Leg4 horizontal servo ' ' Replace these constants with the constants ' you found using the TuneHex.bs2 program. ' ' NOTE: ' You can use this program to adjust the legs centers. Simply remove ' the comment from... ' 'ptrEEPROM = Neutral ' ... then update the center values for each leg. ' ' Example HexCrawler center constants (yours will vary) '-----[ Horizontal Leg Constants ]---------------------------------------- 'Center1 CON 725 'Center2 CON 800 'Center3 CON 785 'Center4 CON 750 'Center5 CON 750 'Center6 CON 750 ' '========================================================================= ' Program Operation Overview '========================================================================= ' VARIABLES AND CONSTANTS ' EEPROM DATA ' MAIN ' ' VARIABLES AND CONSTANTS: ' All constants and variable that control joint position and speed are ' adjustable. ' ' Stride is the distance +/- a horizontal servo moves from center. ' Therefore, a stride of 150 is 150 + center and 150 - center for a ' total of 300 units or about 54 degrees. In version 1.0 you could ' change the stride the value to produce different horizontal swings. ' AND you can still do that but I suggest leaving Stide at 200 and ' using the limiter variable to reduce the horizontal swing. ' ' Delay gives the servo time to reach a position. Delay is expressed ' as a ratio of stride or can be hard coded. ' ' The rightRamp and leftRamp variables hold servo rotation speeds ' (ramp) on the left or right side of the robot. Slowing the ' servos on one side of the robot causes the robot to make a gradual ' turn. ' ' LiftRamp controls vertical servo speed. ' ' The limiter variable trims the horizontal swing. ' ' ptrEEPROM is a pointer to EEPROM addresses. Assigning ' ptrEEPROM to "Forward" (ptrEEPROM = Forawrd) places ' the pointer at the starting EEPROM address for ' forward motion. ' ' EEPROM DATA ' EEPROM data is written in four 73 byte blocks; Forward, Back, ' LSpin, and RSpin. Each block of EEPROM data is broken into 3 ' byte sections; servo address, LOWBYTE of servo position, and ' HIGHBYTE of servo position. EEPORM data is READ 3 bytes at a time ' until an $FF (end of data 73th byte) is found. ' ' MAIN: ' Main reads EEPROM, determines timing values, and write position ' commands to the PSC. ' '========================================================================= ' Sub Routines '========================================================================= ' Main: ' The "Main" sub routine initializes ptrEEPROM to point to a ' block of EEPROM data. 3 byte values are read and stored ' in servoAddr, HIGHBYTE.servoPosition, and LOWBYTE.servoPosition ' variables. Main then evaluates the value in servoAddr. ' ' If servoAddr is an odd number a vertical servo is addressed. Vertical ' servo are assigned a very fast ramp value to lift and lower legs ' quickly. ' ' If servoAddr is even Main assigns ramp values (speed) ' depending on leg location, left or right side of the robot. Slowing ' leg speed on one side of the robot cause the robot to make gradual turn ' toward the slow side. ' ' Main then passes, ptrEEPROM, servoAddr, and servoPosition ' variables to the Write_PSC sub routine. ' ' Main reads the EEPROM data until an $FF is found. An $FF ' indicates the end of an EEPROM data block. ptrEEPROM is reset ' and the process repeats. ' ' Write_PSC: ' The "Write_PSC" sub routine writes servoAddr, ramp, and servoPosition ' to the PSC. It waits briefly to allow the servos to reach position ' then control is passed back to main. ' ' Limit_Stride: ' Limit Stride reduces horizontal swing from each servo's center ' constant. Center values where determined when you programmatically ' center the legs. ' ' SetDelay: ' Set_Ramp_Delay calculates ramp and delay values based on the value ' in limiter. ' '========================================================================= ' Step-by-Step: Configure the HexCrawler's direction. '========================================================================= ' Locate the "Initialize:" section of code below. ' ' LOOKUP 0, [Ahead, ' 0 - Ahead ' ForwardRight, ' 1 - ForwardRight ' RightSpin, ' 2 - RightSpin ' BackRight, ' 3 - BackRight ' Backward, ' 4 - Backward ' BackLeft, ' 5 - BackLeft ' LeftSpin, ' 6 - LeftSpin ' ForwardLeft], dir ' 7 - ForwardLeft ' ' Change the zero in the lookup statement to an integer (0-7) ' where 0 is forward and 4 is backward. ' ' "Run" the program. ' '========================================================================= ' Step-by-Step: Change the HexCrawler's Stride Value '========================================================================= ' Find this section of code: '-----[ Stride Calculations ]--------------------------------------------- 'Stride CON 200 ' Stride units ' ' Stride is the distance from center the legs will travel. ' +/-200 = 400 units total. ' A stride of 400 units is very aggressive. Use the limiter ' variable to trim the horizontal stride. ' ' Find the below code 'Initialize: ' #DEFINE debugMode = 0 ' 0 = normal mode ' ' 1 = debug mode ' limiter = 100 ' Init limiter '... ' ' The limiter variable trims the horizontal swing's starting and ending ' points. A limiter value of 75 reduces the horizontal swing by ' 75*2 = 150 units (150units/5.556 deg/unit) = 27 degrees. If limiter ' is larger than the Stride constant the robot will walk in the opposite ' direction. That's a cool side effect. ' ' Why have a limiter and stride values? Limiter is a variable ' and therfore it can be adjusted during run time. Let's assume that ' your controlling the robot with an RC system. You could setup a ' scheme where the moving the the throttle forward increases the robot's ' speed. ' '========================================================================= ' Tripod Walk Forward Graphic Example: '========================================================================= ' ' **Right Side** ' F \ / \ / \ / ' O _\_ _/_ _\_ _/_ _\_ _/_ ' R / 1 \_/ 2 \_/ 3 | / 1 \_/ 2 \_/ 3 | ' W | | | | ' A | _ _ | | _ _ | ' R \_4_/ \_5_/ \_6_| \_4_/ \_5_/ \_6_| ' D \ / \ / \ / ' \ / \ / \ / ' **Left Side** ' '========================================================================= ' Step-by-STEP: Create New Gaits '========================================================================= ' EEPROM data is stored in blocks. The block begins with a label like ' Forward DATA .... and ends with .... $FF (end of data). The information ' between is broken into 3 byte segments. One segments contains servo ' address (one byte) and servo position (2 bytes). One segment of an ' EEPROM block describes the movement of a single leg joint. ' ' Therefore, placing EEPROM segments in some logical order will create ' a walking gait. ' '========================================================================= ' Notes: '========================================================================= ' ' ' ' '------------------------------------------------------------------------- ' -----[ I/O Definitions ]------------------------------------------------ PSC PIN 15 ' PSC module ' Set baud for all STAMP versions #SELECT $STAMP #CASE BS2SX, BS2P Baud CON 1021 + $8000 #CASE BS2PX Baud CON 1646 + $8000 #CASE #ELSE Baud CON 396 + $8000 #ENDSELECT '---- [Walking Variables] ------------------------------------------------ ptrEEPROM VAR Word ' Gait select servoPosition VAR Word ' Servo Position temp VAR Word ' Workspace servoAddr VAR Byte ' Servo addresses ramp VAR Byte ' Ramp used in SEROUT rightRamp VAR Byte ' Right side ramp values leftRamp VAR Byte ' Left side ramp values limiter VAR Byte ' Reduce stride delay VAR Byte ' Wait vel VAR Byte ' Velocity dir VAR Nib ' Direction '-----[ Gait Constants ]-------------------------------------------------- '-----[ Horizontal Leg Constants ]---------------------------------------- ' Adjustable leg centers ' Left Side ' Increase value to move leg forward Center1 CON 750 Center2 CON 750 Center3 CON 750 ' Right Side ' Increase value to move leg back Center4 CON 750 Center5 CON 750 Center6 CON 750 ' Set SetNeutralOn = 1 and run to place the legs in the neutral position. ' Adjust the center values above to tune the robot #DEFINE SetNeutralOn = 0 '-----[ Stride Calculations ]--------------------------------------------- '///////////////////////////////////////////////////////////////////////// ' The stride value determines the maximum value to move a horizontal ' servo from center. So a stride value of 200 means move 200 units ' plus servo center and 200 units minus servo center or 200 + 200 ' units. ' 5.5556 units = 1 degree ' a Stride of 200 = 72 degrees or ' (200*2)/5.556 = 72 '///////////////////////////////////////////////////////////////////////// Stride CON 200 Leg1Center CON Center1 Leg1Forward CON Center1+Stride ' Stride + Leg1 Center Leg1Back CON Center1-Stride Leg2Center CON Center2 Leg2Forward CON Center2+Stride Leg2Back CON Center2-Stride Leg3Center CON Center3 Leg3Forward CON Center3+Stride Leg3Back CON Center3-Stride Leg4Center CON Center4 Leg4Forward CON Center4-Stride Leg4Back CON Center4+Stride Leg5Center CON Center5 Leg5Forward CON Center5-Stride Leg5Back CON Center5+Stride Leg6Center CON Center6 Leg6Forward CON Center6-Stride Leg6Back CON Center6+Stride LiftRamp CON $0 ' Vertical servo ramp EOD CON $FF ' End of data block '----- [Adjustable vertical servo positions] ----------------------------- RaiseRight CON 300 ' Raised and lowered ** LowerRight CON 1200 ' vertical servo values ** RaiseLeft CON 1200 LowerLeft CON 300 '----- [Direction Constants] --------------------------------------------- Ahead CON 0 ForwardRight CON 1 RightSpin CON 2 BackRight CON 3 Backward CON 4 BackLeft CON 5 LeftSpin CON 6 ForwardLeft CON 7 '----- [EEPROM DATA ] ---------------------------------------------------- ' Walk Forward Forward DATA $01,Word RaiseRight, $05,Word RaiseRight, $09,Word RaiseLeft, $02,Word Leg2Back, $06,Word Leg4Back, $0A,Word Leg6Back, $00,Word Leg1Forward,$04,Word Leg3Forward,$08,Word Leg5Forward, $01,Word LowerRight, $05,Word LowerRight, $09,Word LowerLeft, $03,Word RaiseRight, $07,Word RaiseLeft, $0B,Word RaiseLeft, $00,Word Leg1Back, $04,Word Leg3Back, $08,Word Leg5Back, $02,Word Leg2Forward,$06,Word Leg4Forward,$0A,Word Leg6Forward, $03,Word LowerRight, $07,Word LowerLeft, $0B,Word LowerLeft, EOD Back DATA $01,Word RaiseRight, $05,Word RaiseRight, $09,Word RaiseLeft, $02,Word Leg2Forward,$06,Word Leg4Forward,$0A,Word Leg6Forward, $00,Word Leg1Back, $04,Word Leg3Back, $08,Word Leg5Back, $01,Word LowerRight, $05,Word LowerRight, $09,Word LowerLeft, $03,Word RaiseRight, $07,Word RaiseLeft, $0B,Word RaiseLeft, $00,Word Leg1Forward,$04,Word Leg3Forward,$08,Word Leg5Forward, $02,Word Leg2Back, $06,Word Leg4Back, $0A,Word Leg6Back, $03,Word LowerRight, $07,Word LowerLeft, $0B,Word LowerLeft, EOD LSpin DATA $01,Word RaiseRight, $05,Word RaiseRight, $09,Word RaiseLeft, $02,Word Leg2Back, $06,Word Leg4Forward,$0A,Word Leg6Forward, $00,Word Leg1Forward,$04,Word Leg3Forward,$08,Word Leg5Back, $01,Word LowerRight, $05,Word LowerRight, $09,Word LowerLeft, $03,Word RaiseRight, $07,Word RaiseLeft, $0B,Word RaiseLeft, $00,Word Leg1Back, $04,Word Leg3Back, $08,Word Leg5Forward, $02,Word Leg2Forward,$06,Word Leg4Back, $0A,Word Leg6BAck, $03,Word LowerRight, $07,Word LowerLeft, $0B,Word LowerLeft, EOD RSpin DATA $01,Word RaiseRight, $05,Word RaiseRight, $09,Word RaiseLeft, $02,Word Leg2Forward,$06,Word Leg4Back, $0A,Word Leg6Back, $00,Word Leg1Back, $04,Word Leg3Back, $08,Word Leg5Forward, $01,Word LowerRight, $05,Word LowerRight, $09,Word LowerLeft, $03,Word RaiseRight, $07,Word RaiseLeft, $0B,Word RaiseLeft, $00,Word Leg1Forward,$04,Word Leg3Forward,$08,Word Leg5Back, $02,Word Leg2Back, $06,Word Leg4Forward,$0A,Word Leg6Forward, $03,Word LowerRight, $07,Word LowerLeft, $0B,Word LowerLeft, EOD 'Lower and Center all legs Neutral DATA $00,Word Center1, $02,Word Center2, $04,Word Center3, $06,Word Center4, $08,Word Center5, $0A,Word Center6, $01,Word LowerRight, $03,Word LowerRight, $05,Word LowerRight, $07,Word LowerLeft, $09,Word LowerLeft, $0B,Word LowerLeft, EOD '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' The values below are all adjustable. '///////////////////////////////////////////////////////////////////////// Reset_PSC: ' Reset the Rod legs ptrEEPROM = Neutral limiter = 0 GOTO Main Initialize: ' Init values ' 0 = normal mode ' 1 = debug mode #DEFINE debugMode = 0 ' Init limiter limiter = 75 ' Inti velocity vel = 3 LOOKUP 4, [Ahead, ' 0 - Ahead ForwardRight, ' 1 - ForwardRight RightSpin, ' 2 - RightSpin BackRight, ' 3 - BackRight Backward, ' 4 - Backward BackLeft, ' 5 - BackLeft LeftSpin, ' 6 - LeftSpin ForwardLeft], dir ' 7 - ForwardLeft #IF SetNeutralOn #THEN ' Adjust legs centers if ptrEEPROM = Neutral ' SetNeutralOn flag is on limiter = 0 GOTO Main #ENDIF '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' Set direction gait properties ' Set ptrEEPROM to EEPROM block ' Set left and right ramp values ' ' Parameters (dir) ' Return () '///////////////////////////////////////////////////////////////////////// Set_Direction: SELECT dir CASE Ahead ptrEEPROM = Forward GOSUB Equal CASE ForwardRight ptrEEPROM = Forward GOSUB SlowRight CASE RightSpin ptrEEPROM = RSpin GOSUB Equal CASE BackRight ptrEEPROM = Back GOSUB SlowRight CASE Backward ptrEEPROM = Back GOSUB Equal CASE BackLeft ptrEEPROM = Back GOSUB SlowLeft CASE LeftSpin ptrEEPROM = LSpin GOSUB Equal CASE ForwardLeft ptrEEPROM = Forward GOSUB SlowLeft ENDSELECT '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' Set delay ' Delay provides time for the servos to reach a position and is based ' on ramp (vel). ' This is an adjustable value. ' ' Parameters (vel) ' Return (delay) '///////////////////////////////////////////////////////////////////////// SetDelay: IF vel < 9 THEN delay = 30 ELSE delay = 10 ENDIF '_________________________________________________________________________ '#IF debugMode #THEN 'GOSUB Trace '#ENDIF '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' Main walking engine ' ' Parameters (ME) ' Return Nothing '///////////////////////////////////////////////////////////////////////// Main: 'Read 3 bytes from EEPROM READ ptrEEPROM, ServoAddr, servoPosition.LOWBYTE, servoPosition.HIGHBYTE DO WHILE servoAddr <> EOD 'Determine vertical or horizontal servo IF (servoAddr // 2) = 1 THEN ramp = LiftRamp ELSE 'Set right side ramp IF (servoAddr = $00) OR (servoAddr = $02) OR (servoAddr = $04) THEN ramp = rightRamp ENDIF 'Set left side ramp IF(servoAddr = $06) OR (servoAddr = $08) OR (servoAddr = $0A) THEN ramp = leftRamp ENDIF 'Limit horizontal swing if limiter has a value greater than zero IF (limiter > 0) THEN GOSUB Limit_Stride ENDIF ENDIF GOSUB Write_PSC 'Increment pointer and get the next 3 EEPROM values ptrEEPROM = ptrEEPROM + 3 READ ptrEEPROM, ServoAddr, servoPosition.LOWBYTE, servoPosition.HIGHBYTE #IF debugMode #THEN GOSUB Trace #ENDIF LOOP 'Reset pointer IF ptrEEPROM = Neutral + 36 THEN ' Adjust legs #IF SetNeutralOn #THEN ' Adjust legs if END ' End #ELSE PAUSE 500 ' Reset legs GOTO Initialize #ENDIF ELSE ptrEEPROM = ptrEEPROM - 72 ' Reset pointer ENDIF GOTO Main 'Loop forever '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' Write position commands to the PSC ' ' Parameters (ServoAddr, servoPosition, delay) ' Return Nothing '///////////////////////////////////////////////////////////////////////// Write_PSC: SEROUT PSC,Baud,["!SC",ServoAddr, Ramp,servoPosition.LOWBYTE, servoPosition.HIGHBYTE, CR] PAUSE delay RETURN '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' Reduce leg travel by the value in limiter. This allows for run-time ' horizontal leg control. ' ' Parameters (ServoAddr, servoPosition) ' Return (servoPosition) '///////////////////////////////////////////////////////////////////////// Limit_Stride: 'Get joint center value and put it in temp SELECT ServoAddr CASE $00 temp = Center1 CASE $02 temp = Center2 CASE $04 temp = Center3 CASE $06 temp = Center4 CASE $08 temp = Center5 CASE $0A temp = Center6 CASE ELSE 'Do Noting ENDSELECT 'Reduce swing from center by the value in limiter IF servoPosition > temp THEN servoPosition = servoPosition - limiter ELSE servoPosition = servoPosition + limiter ENDIF RETURN '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' Equal ' 'Parameters (vel) 'Return (rightRamp, leftRamp) '///////////////////////////////////////////////////////////////////////// Equal: rightRamp = vel leftRamp = vel RETURN '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' Slow right side ' 'Parameters (vel) 'Return (rightRamp, leftRamp) '///////////////////////////////////////////////////////////////////////// SlowLeft: rightRamp = vel leftRamp = vel + $5 RETURN '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' Slow left side ' 'Parameters (vel) 'Return (rightRamp, leftRamp) '///////////////////////////////////////////////////////////////////////// SlowRight: rightRamp = vel + $5 leftRamp = vel RETURN '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' Writes variables to the debug terminal when DebugMode = 1 ' ' Copy and paste the Debug tool to strategic location in the code. This ' allow you to see what is in each variable in the Trace sub. '///////////////////////////////////////////////////////////////////////// 'DEBUG Tool #IF debugMode #THEN GOSUB Trace #ENDIF ' Write trace information ' to debug terminal #IF debugMode #THEN Trace: DEBUG CLS, ?ramp, ?leftramp, ?rightramp, ?limiter, ?delay, ?ptrEEPROM RETURN #ENDIF '_________________________________________________________________________ '------------------------------------------------------------------------- ' HexCrawler Spin EEPROM Tables ' You can use these tables for an alternative spin movement. 'RTurn DATA $01,Word RaiseRight, $05,Word RaiseRight, $07,Word RaiseLeft, '$02,Word Center2+200,$08,Word Center4, $0A,Word Center6+300, '$00,Word Center1, $04,Word Center3, $06,Word Center4, '$01,Word LowerRight, $05,Word LowerRight, $07,Word LowerLeft, '$03,Word RaiseRight, $09,Word RaiseLeft, $0B,Word RaiseLeft, '$02,Word Center2+100,$08,Word Center4-100,$0A,Word Center6+200, '$00,Word Center1, $04,Word Center3, $06,Word Center4, '$03,Word LowerRight, $09,Word LowerLeft, $0B,Word LowerLeft, '$FF 'LTurn DATA $01,Word RaiseRight, $05,Word RaiseRight, $0B,Word RaiseLeft, '$02,Word Center2+100,$08,Word Center4-100,$04,Word Center6-300, '$00,Word Center1, $06,Word Center3, $0A,Word Center6, '$01,Word LowerRight, $05,Word LowerRight, $0B,Word LowerLeft, '$03,Word RaiseRight, $09,Word RaiseLeft, $07,Word RaiseRight, '$02,Word Center2+200,$08,Word Center4, $04,Word Center6-200, '$00,Word Center1, $06,Word Center3, $0A,Word Center6, '$03,Word LowerRight, $09,Word LowerLeft, $07,Word LowerRight, '$FF