' www.crustcrawler.com ' Copyright 2006 CrustCrawler Inc. ' '------- [ QuadCrawlerBase.BS2 ] ---------------------------------------------- '{$STAMP BS2} '{$PBASIC 2.5} ' ' File....... QuadCrawlerBase.BS2 ' Purpose.... Investigate program parameters. ' Create and test custom walking gaits. ' Author..... CrustCrawler Inc. (Mike Gebhard) ' Help....... http://forum.crustcrawler.com/ ' Started.... 11/03/2005 ' Updated.... 03/28/2007 ' Version.... 2.0 ' ' Hardware ' (1) QuadCrawler Robotic Kit ' (1) Parallax BOE ' (1) Parallax Basic Stamp II ' (1) Parallax PSC ' '========================================================================= ' Updates 03/28/2007 '========================================================================= ' * Set delay to fire after reading each row in the EEPROM tables ' and NOT after reading each column. ' '========================================================================= ' Updates 5/3/2006 '========================================================================= ' * Updated EEPROM tables ' * Changed center values to point to about 20 degrees from center ' '========================================================================= ' 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 '========================================================================= ' QuadCrawlerBase.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/QuadCrawler.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 ' ' Replace these constants with the constants ' you found using the TuneQuad.bs2 program. ' ' NOTE: ' You can use this program to adjust the legs centers. Simply ' SetNeutralOn = 1 in the Section. '-----[ Horizontal Leg Constants ]---- '... ' #DEFINE SetNeutralOn = 1 ' ' ... then update the center values for each leg. ' ' Example QuadCrawler center constants (yours will vary) '-----[ Horizontal Leg Constants ]---------------------------------------- 'Center1 CON 725 'Center2 CON 800 'Center3 CON 785 'Center4 CON 750 ' '========================================================================= ' Program Operation Overview '========================================================================= ' VARIABLES AND CONSTANTS ' EEPROM DATA ' MAIN ' ' VARIABLES AND CONSTANTS: ' All constants and variable that control 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 150 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 (stride/5) 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, set to 1 or 0. ' ' 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 48 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 48th byte) is found. ' ' MAIN: ' Main Sub 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. ' ' Set_Ramp_Delay: ' Set_Ramp_Delay calculates ramp and delay values based on the value ' in limiter. ' '========================================================================= ' Step-by-Step: Configure the QuadCrawler'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 QuadCrawler'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. ' '========================================================================= ' Walk Forward Diagram '========================================================================= ' ' **Right Side** | ' | ' F \ / | / \ ' O _\_ _/_ | _/_ _\_ ' R / 1 \____/ 2 | | / 1 \____/ 2 | ' W | | | | | ' A | ____ | | | ____ | ' R \_3_/ \_4_| | \_3_/ \_4_| ' D \ / | / \ ' \ / | / \ ' '========================================================================= ' Spin Diagram: '========================================================================= ' ' **Right Side** | ' | ' F \ / | / \ ' O _\_ _/_ | _/_ _\_ ' R / 1 \____/ 2 | | / 1 \____/ 2 | ' W | | | | | ' A | ____ | | | ____ | ' R \_3_/ \_4_| | \_3_/ \_4_| ' D / \ | \ / ' / \ | \ / ' ' '========================================================================= ' Step-by-STEP: Create New Gaits '========================================================================= ' EEPROM data is stored in blocks. The block begins with a label like ' Forward DATA .... and ends with .... EOD (end of data). The information ' between is broken into 3 byte segments. A 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 rowCounter VAR Nib ' Count EERROM rows '-----[ Horizontal Leg Constants ]---------------------------------------- ' Adjustable leg centers ' Right Side - Increase value to move leg forward Center1 CON 750 'Front Center2 CON 750 'Back 'Left Side Center3 CON 750 'Front Center4 CON 800 'Back ' Set SetNeutralOn = 1 and run to place the legs in the neutral position. ' Adjust the center values above to tune the Quad #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 LiftRamp CON $1 ' Vertical servo ramp '----- [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 '---- [ Speed Constants ]------------------------------------------------- Speed1 CON $3 ' Quickest Speed2 CON $7 Speed3 CON $9 Speed4 CON $A Speed5 CON $15 ' Slowest '---- [ App Constants ]--------------------------------------------------- EOD CON $FF ' End of data block '----- [EEPROM DATA ] ---------------------------------------------------- ' 'Right 'Left Forward DATA $01,Word RaiseRight, $07,Word RaiseLeft, $00,Word Leg1Forward, $06,Word Leg4Forward, 'Reset $02,Word Leg2Back, $04,Word Leg3Back, 'Thrust $01,Word LowerRight, $07,Word LowerLeft, $03,Word RaiseRight, $05,Word RaiseLeft, $00,Word Leg1Back, $06,Word Leg4Back, 'Thrust $02,Word Leg2Forward, $04,Word Leg3Forward, 'Reset $03,Word LowerRight, $05,Word LowerLeft, EOD ' end of Forward Back DATA $01,Word RaiseRight, $07,Word RaiseLeft, $00,Word Leg1Back, $06,Word Leg4Back, 'Reset $02,Word Leg2Forward, $04,Word Leg3Forward, 'Thrust $01,Word LowerRight, $07,Word LowerLeft, $03,Word RaiseRight, $05,Word RaiseLeft, $00,Word Leg1Forward, $06,Word Leg4Forward, 'Thrust $02,Word Leg2Back, $04,Word Leg3Back, 'Reset $03,Word LowerRight, $05,Word LowerLeft, EOD ' end of Back ' Left Turn EEPROM values LSpin DATA $01,Word RaiseRight, $07,Word RaiseLeft, $00,Word Leg1Forward, $06,Word Leg4Back, 'Reset $02,Word Leg2Back, $04,Word Leg3Forward, 'Thrust $01,Word LowerRight, $07,Word LowerLeft, $03,Word RaiseRight, $05,Word RaiseLeft, $00,Word Leg1Back, $06,Word Leg4Forward, 'Thrust $02,Word Leg2Forward, $04,Word Leg3Back, 'Reset $03,Word LowerRight, $05,Word LowerLeft, EOD ' end of LSpin ' Right turn EEPROM Values RSpin DATA $01,Word RaiseRight, $07,Word RaiseLeft, $00,Word Leg1Back, $06,Word Leg4Forward, 'Thrust $02,Word Leg2Forward, $04,Word Leg3Back, 'Reset $01,Word LowerRight, $07,Word LowerLeft, $03,Word RaiseRight, $05,Word RaiseLeft, $00,Word Leg1Forward, $06,Word Leg4Back, 'Reset $02,Word Leg2Back, $04,Word Leg3Forward, 'Thrust $03,Word LowerRight, $05,Word LowerLeft, EOD ' end of RSpin ' Raise Center Lower Neutral DATA $01,Word RaiseRight, $00,Word Center1, $01,Word LowerRight DATA $03,Word RaiseRight, $02,Word Center2, $03,Word LowerRight DATA $05,Word RaiseLeft, $04,Word Center3, $05,Word LowerLeft DATA $07,Word RaiseLeft, $06,Word Center4, $07,Word LowerLeft, EOD ' end of adjust legs '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' 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 = 100 ' Inti velocity vel = 3 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 #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 '_________________________________________________________________________ ' View trace if debugMode is on '#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) THEN ramp = rightRamp ENDIF 'Set left side ramp IF(servoAddr = $04) OR (servoAddr = $08) 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 ' View trace if debugMode is on #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 - 48 ' 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] IF rowCounter = 1 THEN PAUSE delay ENDIF 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 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 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 '_________________________________________________________________________