' www.crustcrawler.com ' Copyright 2006 CrustCrawler Inc. ' '------- [ QuadCrawlerPing.bs2 ] ------------------------------------------------- '{$STAMP BS2} '{$PBASIC 2.5} ' ' File....... QuadCrawlerPing.bs2 ' Purpose.... Simple obstacle avoidance with the QuadCrawler and PING))) ' ultra sonic sensor. ' Author..... CrustCrawler Inc. (Mike Gebhard) ' Help....... http://forum.crustcrawler.com/ ' Started.... 05/3/2006 ' Updated.... 03/28/2007 ' Version.... 2.0 ' ' Hardware ' (1) QuadCrawler Robotic Kit ' (1) Parallax BOE ' (1) Parallax Basic Stamp II ' (1) Parallax PSC ' (1) Parallax PING))) Ultra Sonic Sensor ' '========================================================================= ' Updates 03/28/2007 '========================================================================= ' * Set delay to fire after reading each row in the EEPROM tables ' and NOT after reading each column. ' '========================================================================= ' Overview '========================================================================= ' Demo obstacle avoidance using a Parallax PING))) ultra sonic ' distance detector attached to the front of a QuadCrawler. ' ' Distance Detected Action ' 00 to 10 cm Backup ' 11 to 15 cm Spin ' 16 to 30 cm Gradual turn ' 31 to ... Forward '========================================================================= ' Getting Started '========================================================================= ' PING Connections: ' PING BOE '------ ----- ' GND Vss ' 5V Vdd ' SIG Pin 14 ' ' Please read all documentation and review demo/base code for the ' QuadCrawler, PSC, BOE/BS2, and PING))). The code below is a combination ' the QuadCrawlerBase.bs2 and the PING demo application. ' '========================================================================= ' Sub Routines '========================================================================= ' Please refer to QuadCrawlerBase.bs2 and Ping_Demo.bs2 for details. ' ' '------------------------------------------------------------------------- ' -----[ I/O Definitions ]------------------------------------------------ PSC PIN 15 ' PSC module Ping PIN 14 ' PING))) ' ----[ 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 ' -----[ PING))) Constants ]---------------------------------------------- #SELECT $STAMP #CASE BS2, BS2E Trigger CON 5 ' trigger pulse = 10 uS Scale CON $200 ' raw x 2.00 = uS #CASE BS2SX, BS2P, BS2PX Trigger CON 13 Scale CON $0CD ' raw x 0.80 = uS #CASE BS2PE Trigger CON 5 Scale CON $1E1 ' raw x 1.88 = uS #ENDSELECT RawToIn CON 889 ' 65536 / 73.746 (with **) RawToCm CON 2257 ' 65536 / 29.034 (with **) IsHigh CON 1 ' for PULSOUT IsLow CON 0 RefreshConv CON 50 ' Pause = refresh*RefreshConv '---- [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 vel VAR Byte ' Value from speed screen rightRamp VAR Byte ' Right side ramp values leftRamp VAR Byte ' Left side ramp values limiter VAR Byte ' Reduce stride delay VAR Byte ' Wait rowCounter VAR Nib ' Count EERROM rows ' -----[ Variables ]------------------------------------------------------- dist VAR Word ' distance measurement counter VAR Byte ' Loop counter char VAR Byte ' Character sent to LCD dir VAR Nib ' Direction '-----[ 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 '-----[ 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 '---- [ 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, ' $02,Word Leg2Forward, $04,Word Leg3Forward, ' $01,Word LowerRight, $07,Word LowerLeft, $03,Word RaiseRight, $05,Word RaiseLeft, $00,Word Leg1Forward, $06,Word Leg4Forward, ' $02,Word Leg2Back, $04,Word Leg3Back, ' $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, ' $02,Word Leg2Back, $04,Word Leg3Forward, ' $01,Word LowerRight, $07,Word LowerLeft, $03,Word RaiseRight, $05,Word RaiseLeft, $00,Word Leg1Back, $06,Word Leg4Forward, ' $02,Word Leg2Forward, $04,Word Leg3Back, ' $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, ' $02,Word Leg2Forward, $04,Word Leg3Back, ' $01,Word LowerRight, $07,Word LowerLeft, $03,Word RaiseRight, $05,Word RaiseLeft, $00,Word Leg1Forward, $06,Word Leg4Back, ' $02,Word Leg2Back, $04,Word Leg3Forward, ' $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 '_________________________________________________________________________ ' 0 = normal mode ' 1 = debug mode #DEFINE debugMode = 0 Initialize: delay = 10 ' Initialize stride limiter = 0 ' Initialize limiter to 0 dir = Ahead ' direction = forward vel = 1 ' Set ramp to 1 ptrEEPROM = Neutral ' Center and lower all legs GOTO Main ' Center legs '///////////////////////////////////////////////////////////////////////// ' 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 'PAUSE 2000 '#ENDIF '///////////////////////////////////////////////////////////////////////// ' Main walking engine ' ' Parameters () ' Return () '///////////////////////////////////////////////////////////////////////// Main: 'Read 3 bytes from EEPROM READ ptrEEPROM, ServoAddr, servoPosition.LOWBYTE, servoPosition.HIGHBYTE DO WHILE servoAddr <> $FF 'Vertical servo are odd numbers IF (servoAddr // 2) = 1 THEN ramp = LiftRamp ELSE 'Set right side ramp (servo speed) 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 variable 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 IF ptrEEPROM = Neutral + 36 THEN ' First run centers legs limiter = 75 ' Set limiter PAUSE 500 ' Pause ENDIF Get_Ping_Reading: GOSUB Get_Sonar ' Get raw PING reading GOSUB C_Centimeter ' Convert to centimeter GOSUB Determine_Gait_From_Distance GOTO Main 'Loop forever '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' Walk backward if an object is within 10cm ' Spin left if an object is withing 10-15cm ' Make a gradual left turn if an object is detected between 15 and 30cm ' Walk forward if no object is detected within 30cm ' 'Parameters (distance reading) 'Return (dir) '///////////////////////////////////////////////////////////////////////// Determine_Gait_From_Distance: SELECT dist CASE < 10 ptrEEPROM = Back CASE < 15 ptrEEPROM = LSpin CASE < 30 ptrEEPROM = Forward GOSUB SlowLeft CASE ELSE ptrEEPROM = Forward GOSUB Equal ENDSELECT RETURN '///////////////////////////////////////////////////////////////////////// ' 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 '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' This subroutine triggers the Ping sonar sensor and measures ' the echo pulse. The raw value from the sensor is converted to ' microseconds based on the Stamp module in use. This value is ' divided by two to remove the return trip -- the result value is ' the distance from the sensor to the target in microseconds. ' --John Williams "Servo October 2005" '///////////////////////////////////////////////////////////////////////// Get_Sonar: Ping = IsLow ' make trigger 0-1-0 PULSOUT Ping, Trigger ' activate sensor PULSIN Ping, IsHigh, dist ' measure echo pulse dist = dist */ Scale ' convert to uS dist = dist / 2 ' remove return trip RETURN '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' Convert distance reading to centimeter ' ' Parameters (dist) ' Return (dist) '///////////////////////////////////////////////////////////////////////// C_Centimeter: dist = dist ** RawToCm ' convert to centimeters RETURN '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' Convert distance reading to inches ' 'Parameters (dist) 'Return (dist) '///////////////////////////////////////////////////////////////////////// C_Inches: dist = dist ** RawToIn ' convert to inches RETURN '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// 'Parameters (vel) 'Return (rightRamp, leftRamp) ' ' Equal '///////////////////////////////////////////////////////////////////////// Equal: rightRamp = vel leftRamp = vel RETURN '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// 'Parameters (vel) 'Return (rightRamp, leftRamp) ' ' Slow right side '///////////////////////////////////////////////////////////////////////// SlowLeft: rightRamp = vel leftRamp = vel + $5 RETURN '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// 'Parameters (vel) 'Return (rightRamp, leftRamp) ' ' Slow left side '///////////////////////////////////////////////////////////////////////// SlowRight: rightRamp = vel + $5 leftRamp = vel 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 '_________________________________________________________________________ '///////////////////////////////////////////////////////////////////////// ' 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, ?dir, ?vel, ?limiter, ?ptrEEPROM, ?dist RETURN #ENDIF '_________________________________________________________________________