'{$STAMP BS2} '{$PBASIC 2.5} ' Authors note: I do not warrant this softare ' ************************** ' * use at your own risk * ' ************************** ' This software is supplied on an 'as is' basis. It is not copyrighted. ' Maplegate Media and ROBOT magazine have made no contribution and bear no liability. ' The author bears no liablility of any kind, implicit or implied, nor warranty, or responsibility, ' financially OR moraly in the event of damage or destruction of any form of personal property or injury ' of any kind to persons or animals. ' The intent of this softare is for demonstration and education purposes only. ' This software may not be sold, but may be reproduced for the purpose stated above. ' -----[ Variables ]------------------------------------------------- right1x VAR Nib ' variables for leg port assignment right1y VAR Nib right2x VAR Nib right2y VAR Nib right3x VAR Nib right3y VAR Nib left1x VAR Nib left1y VAR Nib left2x VAR Nib left2y VAR Nib left3x VAR Nib left3y VAR Nib counter VAR Byte ' general purpose counter leg VAR Nib ' leg number holder pw VAR Word ' Pulse Width for servo positioning ra VAR Nib ' servo speed ramp number rawForce VAR Word ' the raw output from force sensor sensorPin VAR Word ' sensor pin to select each pressure sensing pad holding VAR Word ' Time counter for distance measurement Distance VAR Word(3) ' Distance holding array direction VAR Nib ' 0 = turn left, 1 = run straight, 2= turn right x VAR Nib ' x is x ' -----[ Constants ]------------------------------------------------- baud CON 396 ' constant for 2400 baud Trigger CON 5 ' trigger pulse = 10 uS Scale CON $200 ' raw x 2.00 = uS IsHigh CON 1 ' for PULSOUT IsLow CON 0 ' for PULSOUT RawToCm CON 2257 ' converts microseconds to centimeters Sdat CON 15 ' port for servo board communications ' ----- [ Pin Assignments ] ----------------------------------------- PingLeft PIN 11 ' Ping module on left side PingMiddle PIN 13 ' Ping module in middle PingRight PIN 14 ' Ping module on right side ' -----[ Variable Initialization ]----------------------------------- ' this is based on my arrangement of servos ' y servo is up and down ' x servo is forward and backward ' looking down on robot, the numbering scheme by name ' front of Khan ' left1x,left1y ------ [] ------ right1x, right1y ' left2x left2y ------ [] ------ right2x, right2y ' left3x left3y ------ [] ------ right3x, right3y ' rear of Khan ' or by channel... ' front of Khan ' 7 8 ------ [] ------ 1 2 ' 9 10 ------ [] ------ 3 4 ' 11 12 ------ [] ------ 5 6 ' rear ' this may differ from your build. check the assignments carefully right1x = 1 ' assign channel values to leg names right1y = 2 right2x = 3 ' assign values to leg names right2y = 4 right3x = 5 ' assign to leg right3y = 6 left1x = 7 ' assign values to leg names left1y = 8 left2x = 9 ' assign channel values to leg names left2y = 10 left3x = 11 ' assign values to leg names left3y = 12 ' assign values to leg names ra = 6 ' servo ramp value counter = 0 ' general purpose counter direction = 1 ' set initial walking direction to middle ' -----[ Main Routine ]----------------------------------------------- GOSUB StandUp ' get the legs organized and the robot upright PAUSE 2000 ' This also sets up a starting position for all the variables ' The main routine is simple, butkeep the following in mind: ' there is no stop. no collision detection ' the robot will walk continuously attempting to avoid obsticles as it does so ' future upgrades include whiskers in front for collision detection ' adjustment to uneven terrain is dynamic. If the robot behaves oddly, unplug the ' pressure sensors. You may need to fiddle with the threshold numbers in the ' LegDown subroutine to get smooth operation. ' main routine main: GOSUB Look ' trigger the Ping)) sensors to scan the area in front GOSUB Decide ' tend to move away from near objects, toward open spaces ' this subroutine also sets the direction variable that adjusts the ' leg movement rates to allow for turns GOSUB walk ' move the robot, turning according to direction variable GOTO main ' repeat END ' end main routine ' -----[ Subroutines ]----------------------------------------------------- ' Trigger the PING device, record the return time and convert the result Look: FOR x = 0 TO 2 ' step through each SELECT x CASE 0 PingLeft = IsLow ' make trigger 0-1-0 PULSOUT PingLeft, Trigger ' activate sensor CASE 1 PULSOUT PingMiddle, Trigger ' activate sensor PingMiddle = IsLow ' make trigger 0-1-0 CASE 2 PingRight = IsLow ' make trigger 0-1-0 PULSOUT PingRight, Trigger ' activate sensor ENDSELECT PULSIN PingLeft, IsHigh, Distance(x) ' measure return and store result in array Distance(x) = Distance(x) */ Scale ' adjust to processor Distance(x) = Distance(x) / 2 ' remove return distance Distance(x) = Distance(x) ** RawToCm ' convert to cm NEXT RETURN ' Compare the values in Distance, and find the highest one, assign the position to direction ' Decide: distance = 0 FOR x = 0 TO 2 ' step through distance array IF Distance(x)> holding THEN ' find the largest distance counter = x ' store array position holding = Distance(x) ' store distance ENDIF NEXT ' increment x direction = counter ' apply array position to variable 'direction' RETURN Walk: ' move 1 leg = left1y ' assign leg to move GOSUB LegUp ' Lift up leg leg = right2y ' assign next leg to move GOSUB LegUp ' lift up leg leg = left3y ' assign next leg GOSUB LegUp ' lift up leg PAUSE 250 ' give the legs enough time to move ' move 2 leg = left1x ' assign leg to move GOSUB LegFore leg = right2x GOSUB LegFore leg = left3x GOSUB LegFore PAUSE 300 ' give the legs enough time to move ' move 3 leg = left1y ' put lifted legs down GOSUB LegDown leg = right2y GOSUB LegDown leg = left3y GOSUB LegDown ' move 4 leg = right1y GOSUB LegUp leg = left2y GOSUB LegUp leg = right3y GOSUB LegUp PAUSE 250 ' give the legs enough time to move ' move 5 leg = left1x GOSUB LegBack leg = right2x GOSUB Legback leg = left3x GOSUB LegBack PAUSE 250 ' give the legs enough time to move ' mvoe 6 leg = right1x GOSUB LegFore leg = left2x GOSUB LegFore leg = right3x GOSUB LegFore PAUSE 300 ' give the legs enough time to move 'move 7 leg = right1y GOSUB LegDown leg = left2y GOSUB LegDown leg = right3y GOSUB LegDown ' move 8 leg = left1y ' assign leg to move GOSUB LegUp ' Lift up leg leg = right2y ' assign next leg to move GOSUB LegUp ' lift up leg leg = left3y ' assign next leg GOSUB LegUp ' lift up leg PAUSE 250 ' give the legs enough time to move ' move 9 leg = right1x GOSUB LegBack leg = left2x GOSUB LegBack leg = right3x GOSUB LegBack PAUSE 250 ' give the legs enough time to move ' note: the robot remains stable and poised for another step. ' call StandUp if you want Khan to stand still RETURN ' Depending on your servos, these values will differ LegDown: ' values may vary, experiment here pw = 750 IF leg = 2 OR leg = 4 OR leg = 6 THEN ' code for right side legs rawForce = 0 DO WHILE PW > 400 AND rawForce < 15000 ' loop while leg is not full down and pressure is low IF PW > 400 THEN ' if the leg is not full down PW = PW - 75 ' subtract from PW to set leg lower SEROUT Sdat, Baud+$8000,["!SC", leg, ra, pw.LOWBYTE, pw.HIGHBYTE, CR] ' move leg GOSUB measure ' check for pressure ELSE PW = 400 ' set full down position right side SEROUT Sdat, Baud+$8000,["!SC", leg, ra, pw.LOWBYTE, pw.HIGHBYTE, CR] ' Move leg ENDIF LOOP ELSE ' code for left side legs rawForce = 0 DO WHILE PW < 1099 AND rawForce < 15000 ' loop while leg is not full down and pressure is low IF PW < 1100 THEN ' if the leg is not full down... PW = PW + 75 ' add to PW to set leg lower SEROUT Sdat, Baud+$8000,["!SC", leg, ra, pw.LOWBYTE, pw.HIGHBYTE, CR] ' Move leg GOSUB measure ' check pressure ELSE pw = 1100 ' set full down position left side SEROUT Sdat, Baud+$8000,["!SC", leg, ra, pw.LOWBYTE, pw.HIGHBYTE, CR] ' Move leg ENDIF LOOP ENDIF RETURN ' called by legdown ' puts the value of the sensor in rawForce Measure: HIGH sensorPin ' charge the capacitor sensorPin = leg ' sensor ports match leg numbers PAUSE 1 RCTIME sensorPin,1,rawForce ' Measure RC discharge time, assign the value to rawForce RETURN ' move all legs down to standing position StandUp: leg = 1 ' Starting servo DO WHILE leg < 13 ' step through all the servos IF leg = 1 OR leg = 3 OR leg = 5 OR leg = 7 OR leg = 9 OR leg = 11 THEN pw = 750 ' center the leg fore-aft before putting it down SEROUT Sdat, Baud+$8000,["!SC", leg, ra, pw.LOWBYTE, pw.HIGHBYTE, CR] ELSE GOSUB LegDown ' put the leg down ENDIF leg = leg + 1 ' increment counter to go through all servos LOOP RETURN ' lifts up the specified leg LegUp: IF leg = 2 OR leg = 4 OR leg = 6 THEN pw = 880 ELSE pw = 600 ENDIF SEROUT Sdat, Baud+$8000,["!SC", leg, ra, pw.LOWBYTE, pw.HIGHBYTE, CR] RETURN ' This routine looks funny because some of my servos are reversed ' you will need to alter this code to match your servo direction ' also, the value is determined from experimentation and is not a fixed number ' Between LegFore and LegBack, the net effect is to stop the legs at center on the ' side that the robot is intended to turn to LegFore: IF leg = 3 THEN pw = 550 ELSE pw = 950 ENDIF IF direction = 0 THEN IF Leg = 7 OR leg = 9 OR leg = 11 THEN pw = 750 ' reduce the forward motion the left legs to turn left ENDIF ELSEIF direction = 2 THEN IF Leg = 1 OR leg = 3 OR Leg = 5 THEN pw = 750 ' reduce the forward motion the right legs to turn right ENDIF ENDIF SEROUT Sdat, Baud+$8000,["!SC", leg, ra, pw.LOWBYTE, pw.HIGHBYTE, CR] RETURN ' This routine also looks funny because some of my servos are reversed ' you will need to alter this code to match your servo direction ' also, the value is determined from experimentation and is not a fixed number LegBack: IF leg = 3 THEN pw = 950 ELSE pw = 550 ENDIF IF direction = 0 THEN ' direction variable value = left turn IF Leg = 7 OR leg = 9 OR leg = 11 THEN ' check legs on left side pw = 750 ' reduce the backward motion the left side legs to turn left ENDIF ELSEIF direction = 2 THEN ' direction value = right turn IF Leg = 1 OR leg = 3 OR Leg = 5 THEN ' if a right side leg, then pw = 750 ' reduce the backward motion the right legs to turn right ENDIF ENDIF SEROUT Sdat, Baud+$8000,["!SC", leg, ra, pw.LOWBYTE, pw.HIGHBYTE, CR] RETURN ' these routines are little ones I used during developement to align legs and check ranges ' While these are not called by the main program, you may want to put temporary code in to ' check the same things, which are likely to differ on your version SitDown: leg = 2 DO WHILE leg < 13 ' GOSUB LegUpHalf GOSUB LegUp leg = leg + 2 LOOP RETURN AllForward: direction = 1 leg = 1 DO WHILE leg < 13 GOSUB LegFore leg = leg + 2 LOOP RETURN AllBack: direction = 1 leg = 1 DO WHILE leg < 13 GOSUB LegBack leg = leg + 2 LOOP RETURN LegUpHalf: IF leg = 2 OR leg = 4 OR leg = 6 THEN pw = 600 ELSE pw = 900 ENDIF SEROUT Sdat, Baud+$8000,["!SC", leg, ra, pw.LOWBYTE, pw.HIGHBYTE, CR] RETURN RETURN LegCenter: FOR leg = 1 TO 12 STEP 2 pw = 750 SEROUT Sdat, Baud+$8000,["!SC", leg, ra, pw.LOWBYTE, pw.HIGHBYTE, CR] NEXT RETURN CenterAll: pw = 750 FOR leg = 1 TO 12 SEROUT Sdat, Baud+$8000,["!SC", leg, ra, pw.LOWBYTE, pw.HIGHBYTE, CR] NEXT PAUSE 1000 RETURN