'{$STAMP BS2} '{$PBASIC 2.5} '>TF-Condor.BS2 '>dynamic.BS2 dynamic walking '>TecFoot 27 July 2000 Milford Instruments 'code by David Buckley DEBUG CLS,"2.4" 'Control program for TecFoot with Stamp2 and two co-processor units. 'History '------- '30Mar09 added DefaultSspeed CON '02Oct08 2.4 reordered program introduced Cmnd and Default Cmdn in DATA '08Sep08 changed B prefix to B_ '13Jul08 2.3 changed to Getcmnd->Walk->Getcmnd... instead of Walk->Gosub(Getcmnd) ' Paces added '26Jun08 prefixed setup names with B(ehavior) '26Jun08 added Obstacle vars to remember R/L obstacles, to get out of corners '26Jun08 added usXin<6 then sbk, now can go backwards away from objects '26Jun08 ver 2.2 '03Mar08 removed RockPause and added 80mS footdown pause for really foot down '02Mar08 added balanceR and balanceL variables for feet, and pot speed control '14Feb08 added RevEd-LCD to see what US distances are '13Feb08 added toe sensors '------------------------------------------- 'Servo co-processor info: 'Send two bytes, 'Command byte format: 'XXXXYYZZ 'XXXX is speed 0 to 15 - extra increments per frame (0=slow,15=fast) ' ZZ is servo number - 0,1,2 ' YY is command : ' 00 - set servo end position ' 01 - report servo position ' XXXX ignored, ZZ =servo number ' response =0 to 255 =current servo position. ' 10 - report all servos ' XXXX ignored, ZZ must be %00 ' response = %STUVWXYZ ' where S,T,U,V,W,X,Y and Z may be 0 or 1 ' STUV ' S - (servo 4) always 1 on 3 servo chip ' T - servo 2 ' U - servo 1 ' V - servo 0 ' 0 => relaxed ' 1 => being pulsed ' WXYZ ' W - (servo 4) always 1 on 3 servo chip ' X - servo 2 ' Y - servo 1 ' Z - servo 0 ' 0 => not reached end point ' 1 => has reached end point ' (ie a quick way to read all servos) ' 11 - relax servo, ie stop pulsing it until 00 sent ' byte is from 0 to 255 '[pulse as measured by BS2, 0=>1018us, 255=>2042us ie 4us resolution] '------------------------------------------- 'SERIAL LCD 'T2400 (or T9600 with jumper)(or inverted with jumper) 'jumpers - open/closed = welcome-message/none, 2400/9600, true/inverted, 2-lines/1-line 'precede all codes by 254 =command character ' 1 clear display and move to start of first line ' 2 move cursor and display 'window' to the start of the first line ' 3 seems to be same as 2 ' 4 set 'right to left' printing mode - not useful ' 5 set 'scroll printing to the left' mode - not useful ' 6 set 'left to right' printing mode ' 7 set 'scroll printing to the right' mode ' 8,9 blanks screen, crash, needs power off to reset ' 10 turn visual LCD screen off ' 11 blanks screen, crash, needs power off to reset ' 12 hide cursor ' 13 make cursor flash ' 14 turn visual LCD screen (and cursor) on ' 15 turn on flashing block cursor ' 16 move cursor left one position ' 17,18,19 no discernable effect ' 20 move cursor right one position ' 21,22,23 no discernable effect ' 24 scroll display 'window' left one position, window wraps at 40 chars ' 25,26,27 same as 28 ' 28 scroll display 'window' right one position, window wraps at 40 chars ' 128 move cursor to start of first line, 129 position 1 etc (window 0-39) ' 192 move cursor to start of second line, 193 position 1 etc (window 0-39) '------------------------------------------- 'Stamp2 connections and constants: ltx CON 0 'serial out pin to left leg servo co-processor lflow CON 1 'flow control for left leg servo co-processor 'pin=0 to prevent tx rtx CON 2 'serial line for right leg servo co-processor rflow CON 3 'flow control for right leg servo co-processor 'pin=0 to prevent tx PingR PIN 4 PingL PIN 5 spkr PIN 6 'Piezo speaker via 0u1 pot CON 7 '220R pin to (10K pot in parallel with 0u1) to 0v [0-603] 'Pins 8 - 15 all pulled up to BS2+5v by 10K footsensR PIN 8 '100K pullup, 0u1 to 0v, switched to 0v by ground force footToeR PIN 9 '10K pullup 100Uf+100uF to 0v, toe uswitch to 0v across caps 'pin 10 - to pad D0 'pin 11 - to pad D1 'pin 12 - LCD and via 220R to pad D2 LCD CON 12 'use no welcome msg jumper1 then no cls needed PadOrUS_ PIN 13 '- to pad, jumpered to gnd, pulled high by 10K TO Reg-5v, Low=>sonar footToeL PIN 14 '10K pullup, 100Uf+100uF to 0v, toe uswitch to 0v across caps footsensL PIN 15 '100K pullup, 0u1 to 0v, switched to 0v by ground force 'Ping same as servo o 0v ' o 5v from 78L05 regulator each Ping takes 22mA quiescent ' o BS2 pin ' 'LCD same as servo o 0v ' o 5v from 78L05 regulator ' o BS2 pin ' Mode jumper connect for U/S, open for Pad for manual control ' o BS2-Pin13 ' o 0v ' ' Pad connector 'read into Cmnd ' o D2 220R to LCD and BS2-Pin12 ' o D1 Pin11 ' o D0 Pin10 ' 1 o 0v ' BRANCH Cmnd,[Start,Brock,Bfd,Bbk,Brt,Blt] 'set up behaviour for walk ' 0 = stop ' 1 = Brock ' 2 = Bfd ' 3 = Bbk ' 4 = Brt ' 5 = Blt ' ' Aux connector 2 ' E=0v Pins - all pulled up to BS2+5v by 10K ' 0v o o 8 - footsensR ' 0v o o 9 - footToeR ' Proto 6v o o 10 - BS2 padD0 ' type 6v o o 11 - side padD1 ' side 9v o o 12 - padD2/LCD ' 9v o o 13 - Pad/US_ jumpered to 0v US ' 5v o o 14 - footToeL ' 5v o o 15 - footsensL ' 'Shoulder pads and Feet ' right left ' foot ------- 0v foot 5v - 330R - LED -O- switch - 0v ' switches D0 D7 switches ^ ' D2 D3 D1 D6 D7 D6 1N914 (so 330R doesn't charge 0u1) ' D2 D5 | ' D0 D1 D3 D4 D5 D4 5v -100K -O- 0u1 - 0v ' - - | ' Reg-5v ------- Pin - 1N914 >-- 220R - ' front (diode OR) ' 'Left foot diode ORed to P15 'Right foot diode ORed to P8 ' 'LCD RevEd T9600 CON 84 'jumper2 9600 N9600 CON 16468 'jumper2 9600 i96n CON 16468 'jumper2 9600 'LCD CON 10 'use no welcome msg jumper1 then no cls needed '------------------------------------------- 'Ping rawDist VAR Word 'for Ping uS2inch CON 889 ' 1 / 73.746 (with **) uS2cm CON 2257 ' 1 / 29.034 (with **) usRin VAR Byte 'U/S range R in inches, so fits in a byte usLin VAR Byte 'U/S range L in inches, so fits in a byte '------------------------------------------- Sbaud CON 16468 '9600 Sbaud, 8 bit, inverted Sservset CON 0 Sservread CON %00000100 SQstatus CON %00001000 'command byte to get status of all servos Scmnd VAR Byte Sspeed VAR Scmnd.HIGHNIB Sservo VAR Scmnd.LOWNIB Spos VAR Byte Sstatus VAR Byte 'Servo status flags 'Sflow VAR Nib 'data line 'Stx VAR Nib 'flow control line footup CON 1 'all switches ORed open read high foot CON 0 'servo number leg CON 1 'servo number splay CON 2 'servo number SQueryleg CON %0101 'command value to query servo 1 = leg true CON 1 false CON 0 IsHigh CON 1 ' for PULSOUT IsLow CON 0 ' for PULSOUT ObstaclesMap VAR Byte ObL VAR ObstaclesMap.HIGHNIB ObR VAR ObstaclesMap.LOWNIB ObAvoid VAR Nib ObMax CON 4 'pot in circuit and not set to zero f_leanR VAR Bit '1=>lean R AND move LeftFoot fdlean VAR Byte lstep VAR Byte '1/2 of a stride Rtb VAR Byte 'RightTurnBy Ltb VAR Byte 'LeftTurnBy foottipR VAR Byte 'offset from flat, outside down +ve foottipL VAR Byte 'offset from flat, outside down +ve footbalanceR VAR Byte 'offset from flat, outside down +ve footbalanceL VAR Byte 'offset from flat, outside down +ve waitcount VAR Byte RlegAt VAR Byte LlegAt VAR Byte upwaitmax CON 50 'upwaitmin CON 20 Cmnd VAR Nib i VAR Cmnd 'FOR NEXT counter in RELAX potval VAR Word paces VAR Byte 'scope VAR Byte 'wandering range max for paces 'scope =10 'or set this by Pot 'servos 'pins mirror 'servo =0 rfoot, 255=tip, + out down 'servo =1 rleg, 0=fd, 255=bk 'servo =2 splay, 0=in 'servo =3 lfoot, 0=tip - out down 'servo =4 lleg, 255=fd, 0=bk 'servo =5 spare DefaultSspeed CON 12 'used by checkpot: DefaultCmnd DATA 0 '-------------------- Program ----------------------------------- Init: READ DefaultCmnd,Cmnd IF Cmnd=0 THEN Cmnd =2 ELSE Cmnd =0 ENDIF WRITE DefaultCmnd,Cmnd 'Condor either stands still or walks Fd, either mode is responsive to US, PAUSE 200 'for LCD to initialise SEROUT LCD,T9600,[254,1] PAUSE 2 'for LCD to cls SEROUT LCD,T9600,["TecFoot"] IF PadOrUS_ THEN Padmsg SEROUT LCD,T9600,[" usL usR"] :GOTO Sspeedmsg Padmsg: SEROUT LCD,T9600,[" Pad"] Sspeedmsg: SEROUT LCD,T9600,[254,192,"speed="] GOSUB beepbeep checkpot: 'only called by manual_setup: if required HIGH 7 PAUSE 1 RCTIME 7,1,potval IF potval=0 OR potval>800 THEN f_pot =0 Sspeed =DefaultSspeed 'no pot ELSE f_pot =1 'use pot ENDIF DEBUG CRSRXY,0,1,? potval,? f_pot 'SEROUT LCD,T9600,[254,192,DEC potval] 'SEROUT LCD,T9600,[254,198,DEC Sspeed," "] 'GOTO checkpot 'Development options '------------------- 'GOTO showSen 'GOTO sway 'GOTO manual_setup '-------------------------------------------------------------------- Start: 'GOSUB report 'DEBUG "gC" Rtb =0 Ltb =0 obR =obR MIN obdecay -obdecay obL =obL MIN obdecay -obdecay ObAvoid =ObAvoid MIN obdecay -obdecay IF (ObR+ObL)>ObsMax THEN ObAvoid =ObsMax 'if obstacles trigger avoidOb IF ObAvoid>0 THEN GOSUB beepbeep IF ObL>ObR THEN B_Rt GOTO B_Lt ENDIF 'ReadPot: IF f_pot THEN 'ELSE use DefaultSspeed set in checkpot: HIGH 7 PAUSE 1 RCTIME 7,1,potval Sspeed =potval /40 MAX 15 ENDIF 'DEBUG ?Sspeed SEROUT LCD,T9600,[254,198,DEC Sspeed," "] IF PadOrUS_ THEN ReadPad '---------------------------- ReadSensors: _ReadToes: ' DEBUG ? footToeL ' DEBUG ? footToeR IF footToeL=0 AND footToeR=0 THEN DEBUG " t-bk" ObL =ObL +obup MAX ObMax ObR =ObR +obup MAX ObMax GOTO B_Bk ENDIF IF footToeR=0 THEN DEBUG " t-lt" :ObR =ObR +obup MAX ObMax :GOTO B_Lt IF footToeL=0 THEN DEBUG " t-rt" :ObL =ObL +obup MAX ObMax :GOTO B_Rt 'GOTO DefaultBehaviour 'if not using Sonar _ReadSonar: GOSUB Get_Sonar usRin =usRin MAX 90 'allows mid to be 90 - 164 ie 127 +-37 usLin =usLin MAX 90 _DisplaySonar: SEROUT LCD,T9600,[254,201,DEC3 usLin," ",DEC3 usRin] DEBUG CRSRXY,0,3,"sonar ",DEC usLin,",",DEC usRin," =>",CLREOL _CaseSonar: IF usRin<6 THEN DEBUG "s-bk" :ObR =ObR +obup MAX ObMax :GOTO B_Bk IF usLin<6 THEN DEBUG "s-bk" :ObL =ObL +obup MAX ObMax :GOTO B_Bk IF usRin<15 THEN DEBUG "s-lt" :ObR =ObR +obup MAX ObMax :GOTO B_Lt IF usLin<15 THEN DEBUG "s-rt" :ObL =ObL +obup MAX ObMax :GOTO B_Rt IF usRin>usLin THEN RTb =usRin -usLin *128 /usRin MAX 100 IF usLin>usRin THEN LTb =usLin -usRin *128 /usLin MAX 100 SEROUT LCD,T9600,[254,201,DEC3 usLin," ",DEC3 usRin] 'SEROUT LCD,T9600,[254,201,DEC3 LTb," ",DEC3 RTb] ' sturn =Ltb -Rtb DEBUG CRSRXY,0,4,"Ltb ",DEC Ltb," ","Rtb ",DEC Rtb," ",CR ' DEBUG 1,CR,"Tb ",DEC Ltb,",",DEC RTb," ",CR DefaultBehaviour: READ DefaultCmnd,Cmnd 'Bstop or Bfd alternates on Reset/PowerUp GOTO BehaviourSetup '---------------------------- ReadPad: 'DEBUG"rP" INPUT LCD 'Pin10 Cmnd.BIT0 =IN10^1 'invert bits Cmnd.BIT1 =IN11^1 Cmnd.BIT2 =IN12^1 DEBUG 1,"Pad ",DEC Cmnd," ",BKSP 'these can be displayed by LEDcard ' DEBUG "*",BIN1 footsensR," ",BIN1 footsensL,"*" IF footsensR AND footsensL THEN Start 'wait till one foot down '---------------------------- BehaviourSetup: 'leaves BRANCH Cmnd,[B_Stop,B_Rock,B_Fd,B_Bk,B_Rt,B_Lt] 'set up behaviour for walk GOTO Start 'for Cmnd>5, ie illegal key presses =6,7 'lstep + fdlean must be less than mid to 0 and mid to 255 or wraps! B_Stop: foottipL =0 foottipR =0 footbalanceR =0 'offset from flat, outside down +ve footbalanceL =0 'offset from flat, outside down +ve lstep =0 fdlean =0 RTb =0 LTb =0 GOTO Walk B_Rock: foottipL =80 foottipR =80 footbalanceR =30 'offset from flat, outside down +ve footbalanceL =25 'offset from flat, outside down +ve lstep =0 fdlean =10 GOTO Walk B_Fd: foottipL =88 foottipR =88 footbalanceR =25 'offset from flat, outside down +ve footbalanceL =24 'offset from flat, outside down +ve lstep =90 fdlean =30 GOTO Walk B_Bk: foottipL =88 foottipR =88 footbalanceR =20 'offset from flat, outside down +ve footbalanceL =20 'offset from flat, outside down +ve lstep =-60 fdlean =-50 GOTO Walk B_Rt: footbalanceR =15 'offset from flat, outside down +ve footbalanceL =15 'offset from flat, outside down +ve Rtb =120 GOTO _B_t B_Lt: footbalanceR =15 'offset from flat, outside down +ve footbalanceL =15 'offset from flat, outside down +ve Ltb =120 _B_t: foottipL =88 foottipR =88 lstep =0 fdlean =20 GOTO Walk '----------------------- Dynamic Walk ------------------------------- walk: 'dynamic walk DEBUG CRSRY,5,CLREOL,"W"'alk" footdown CON 100 ' IF f_leanR =1 THEN moveR '#1 moveL: ' f_leanR =1 '#1 GOSUB waitLon 'can't push off unless it's down PAUSE footdown tiptoR: Sservo =foot SEROUT rtx\rflow,Sbaud,[Scmnd,Rfootflat-footbalanceR] 'DEBUG DEC Rfootflat,"," 'DEBUG DEC (Rfootflat-footbalanceR) SEROUT ltx\lflow,Sbaud,[Scmnd,Lfootflat-foottipL] isLoff: GOSUB qmovedL IF Sstatus.LOWNIB<>%1111 THEN isLoff 'wait till servo move done 'foot level left SEROUT ltx\lflow,Sbaud,[Scmnd,Lfootflat] '+ out down oscillates and falls over, + out up won't oscillate ' and swing left forward Sservo =leg SEROUT rtx\rflow,Sbaud,[Scmnd,Rlegmid+lstep+fdlean] SEROUT ltx\lflow,Sbaud,[Scmnd,Llegmid+lstep-fdlean] Sservo =splay SEROUT rtx\rflow,Sbaud,[Scmnd,splay0 +Ltb MAX 255] '+ve => splay out mid: '------------------ mid swing -------------- moveR: ' f_leanR =0 '#1 GOSUB waitRon 'can't push off unless it's down PAUSE footdown tiptoL: Sservo =foot SEROUT rtx\rflow,Sbaud,[Scmnd,Rfootflat+foottipR] SEROUT ltx\lflow,Sbaud,[Scmnd,Lfootflat+footbalanceL] isRoff: GOSUB qmovedR IF Sstatus.LOWNIB<>%1111 THEN isRoff 'wait till servo move done 'foot level right SEROUT rtx\rflow,Sbaud,[Scmnd,Rfootflat] 'and swing right forward Sservo =leg SEROUT rtx\rflow,Sbaud,[Scmnd,Rlegmid-lstep+fdlean] '-ve=forward SEROUT ltx\lflow,Sbaud,[Scmnd,Llegmid-lstep-fdlean] '+ve=forward Sservo =splay SEROUT rtx\rflow,Sbaud,[Scmnd,splay0 +Rtb MAX 255] '+ve => splay out paces =paces +2 GOTO Start '------------------- Subroutines --------------------------------- report: 'DEBUG DEC foottipL," ",DEC waitcountL," ",DEC foottipR," ",DEC waitcountR,CR DEBUG BIN4 footsensL," ",BIN4 footsensR,CR RETURN '---------------------------- 'showSen: DEBUG 1,BIN1 IN12,BIN1 IN13," ",BIN1 IN9, BIN1 IN8,CR DEBUG BIN1 IN14,BIN1 IN15," ",BIN1 IN11,BIN1 IN10,CR ' goto showSen RETURN '---------------------------- Get_Sonar: 'max PULSIN is 18.5mS (Ping Documentation) 'ie 18500uS =>9250 counts@2us 'ie max target is 9250uS =>125.5inch PAUSE 10 PingR = IsLow ' make trigger 0-1-0 PULSOUT PingR,5 ' 10uS pulse, activate sensor PULSIN PingR, IsHigh, rawDist ' measure echo pulse usRin =rawDist ** uS2inch ' use inches then fits in a byte PAUSE 10 PingL = IsLow ' make trigger 0-1-0 PULSOUT PingL,5 ' 10uS pulse, activate sensor PULSIN PingL, IsHigh, rawDist ' measure echo pulse usLin =rawDist ** uS2inch ' use inches then fits in a byte RETURN '---------------------------- waitRon: 'DEBUG"wR" waitcount =0 wR: IF footsensR<>footup THEN endwR 'footup=all off floor waitcount =waitcount+1 PAUSE 10 IF waitcountfootup THEN endwL 'footup=all off floor waitcount =waitcount+1 PAUSE 10 IF waitcount%1111 THEN waitfinished waitfinishedL: SEROUT ltx\lflow,Sbaud,[SQstatus,0] '?status SERIN ltx,Sbaud,50,waitfinishedl,[Sstatus] ' debug bin result,"left",cr IF Sstatus.LOWNIB<>%1111 THEN waitfinishedl RETURN '---------------------------- QwhereR: 'Query position servo has reached SEROUT rtx\rflow,Sbaud,[Scmnd,0] '?status SERIN rtx,Sbaud,50,QwhereR,[Spos] RETURN '---------------------------- QwhereL: 'Query position servo has reached SEROUT ltx\lflow,Sbaud,[Scmnd,0] '?status SERIN ltx,Sbaud,50,QwhereL,[Sstatus] RETURN '---------------------------- QmovedR: 'QueryMovedRight SEROUT rtx\rflow,Sbaud,[SQstatus,0] '?status SERIN rtx,Sbaud,50,Qmovedr,[Sstatus] RETURN '---------------------------- QmovedL: 'QueryMovedLeft SEROUT ltx\lflow,Sbaud,[SQstatus,0] '?status SERIN ltx,Sbaud,50,Qmovedl,[Sstatus] RETURN '---------------------------- beepbeep: FREQOUT spkr, 100, 2500, 3000 PAUSE 100 FREQOUT spkr, 100, 2500, 3000 RETURN '---------------------------------------------------------------- 'isLoff 'IF footsensL<>footup AND Sstatus.LOWNIB<>%1111 THEN isLoff 'if tired this could wait forever 'isRoff: 'IF footsensR<>footup AND Sstatus.LOWNIB<>%1111 THEN isRoff 'if tired this could wait forever '---------------------------------------------------------------- sway: 'simple sway to check servo limits DEBUG"sway" Sspeed =8 'tip Sservo =0 SEROUT rtx\rflow,Sbaud,[Scmnd,170] 'rfoot,255=tip, SEROUT ltx\lflow,Sbaud,[Scmnd,180] 'lfoot, 0=tip,185,210 GOSUB waitfinished 'pause 1000 'foot level right Sservo =0 SEROUT rtx\rflow,Sbaud,[Scmnd,90] 'rfoot,255=tip, 135 SEROUT ltx\lflow,Sbaud,[Scmnd,180] 'lfoot, 0=tip,185,210 GOSUB waitfinished PAUSE 100 ' gosub STANDUP 'tip Sservo =0 SEROUT rtx\rflow,Sbaud,[Scmnd,70] 'rfoot,255=tip, 35, 27 SEROUT ltx\lflow,Sbaud,[Scmnd,80] 'lfoot, 0=tip, GOSUB waitfinished 'pause 1000 'foot level left Sservo =0 SEROUT rtx\rflow,Sbaud,[Scmnd,70] 'rfoot,255=tip, 35, 27 SEROUT ltx\lflow,Sbaud,[Scmnd,160] 'lfoot, 0=tip, 120 GOSUB waitfinished PAUSE 100 ' gosub STANDUP GOTO sway END '---------------------------------------------------------------- manual_setup: 'edit position values to check stances 'Rfootflat CON 125 'us49=124 '+ is down 'Lfootflat CON 131 'us51=120 '+ is up 'Rlegmid CON 128 'us50=128 '-ve fd 'Llegmid CON 128 'us50=128 '+ve fd 'splay0 CON 59 'us23=59 '+ve out Sspeed =8 Sservo =0 ' SEROUT rtx\rflow,Sbaud,[Scmnd,0] '=1018-1020us 'rfoot, 255=tip, 35, 27 'to check pulse length ' SEROUT rtx\rflow,Sbaud,[Scmnd,255] '=2042us 'rfoot, 255=tip, 35, 27 'to check pulse length 'END SEROUT rtx\rflow,Sbaud,[Scmnd,124] 'rfoot, 255=tip, 35, 27 ' SEROUT rtx\rflow,Sbaud,[Scmnd,potval] 'rfoot, 255=tip, 35, 27 SEROUT ltx\lflow,Sbaud,[Scmnd,120] 'lfoot, 0=tip,185,220 ' SEROUT ltx\lflow,Sbaud,[Scmnd,potval] 'lfoot, 0=tip,185,220 Sservo =1 SEROUT rtx\rflow,Sbaud,[Scmnd,128] 'rleg, 0=fd, 255=bk ' SEROUT rtx\rflow,Sbaud,[Scmnd,potval] 'rleg, 0=fd, 255=bk SEROUT ltx\lflow,Sbaud,[Scmnd,128] 'lleg, 255=fd, 0=bk ' SEROUT ltx\lflow,Sbaud,[Scmnd,potval] 'lleg, 255=fd, 0=bk Sservo =2 SEROUT rtx\rflow,Sbaud,[Scmnd,94] 'splay, 0,255=out ' SEROUT rtx\rflow,Sbaud,[Scmnd,potval] 'splay, 0,255=out ' SEROUT ltx\lflow,Sbaud,[Scmnd,127] 'nothing ' GOTO checkpot DEBUG"endtest" END '----------------------------------------------------------------