'{$STAMP BS2e} '{$PBASIC 2.5} 'Tom14-4.BSe DEBUG CR,"{144,",HEX cmnd',"(",DEC cmnd,")",",",HEX param,"(",DEC param,")}@",DEC page,",",HEX pD,CR Aslot =4 'prog0 Command interpreter 'prog1 move 'prog2 talk + word_DATA 'prog3 Acts1_DATA+Ear 'prog4 Acts2_DATA+Ear '------------------------------------------------------------------ 'S_toeR PIN 0 'serial - Right Twinkletoes 'S_toeL PIN 1 'serial - Left Twinkletoes servoroll PIN 2 'servo servopace PIN 3 'servo servosplay PIN 4 'servo 'SRF05 PIN 5 'pin for both trigger and echo Vs is 5v 'voicepin PIN 6 'potpin PIN 7 'user pot 'PicAxe40xTo PIN 8 ' 'PicAxe40xFrom PIN 9 'HexSwitch Act 'EyeRpin PIN 10 ''ORP12 || 10n to 0v, (10,11 on same three pin connector) 'EyeLpin PIN 11 ''ORP12 || 10n to 0v 'PIN 12 'PIN 13 BSeRdy PIN 14 'START: OUT14 =fB_EyesWalkL Hmm, can't plug debug LEDs in Mom, ok for Joe,Tom,Kas EarRdy_ PIN 15 EarListen PIN 15 '-------------------------- rollRat VAR Byte rollRto VAR Byte 'rollC var byte 'used for **determine rollC** section paceLat VAR Byte paceLto VAR Byte splayat VAR Byte splayto VAR Byte turnby VAR Byte turnbyL VAR turnby.HIGHNIB 'turnby 100uS units turnbyR VAR turnby.LOWNIB 'turnby 100uS units pD VAR Byte 'pointer to current move in DATA 'Waddr VAR pD '#2 address of word pDa VAR Byte 'pointer to current move in ActsconaDATA 'Aaddr VAR pDa '#2 address of allophone pDb VAR Byte 'pointer to selected behaviour in bDATA cmnd VAR Byte 'current command Act VAR cmnd 'init:, servo routine SERIN BAct VAR cmnd param VAR Byte 'current command parameter ' temp VAR param 'servo routine tempE VAR param 'local to __EwBehaviour: potval VAR Word 'readpot: getspeed: i VAR potval 'FOR counter Vt eyeRCtime VAR potval 'Readeyes: USrange VAR potval 'US raw range value USawareAt VAR Nib '#0 US Aware Range resolution 2"=>5cm, max 15=>30" UStooNear VAR Nib '#0 US Action Range resolution 2"=>5cm, max 15=>30" USmem VAR Nib '#0 US previous Range resolution 2"=>5cm, max 15=>30" Flags VAR Byte fB_Fdir VAR Flags.BIT0 '1=>Fd 0=>halt 'for _H: and Turn on spot fB_Bdir VAR Flags.BIT1 '1=>Bk 0=>halt 'for Turn on spot fB_tty VAR Flags.BIT2 'tty control, 1=>print, 0=>no printing fB_StOn VAR Flags.BIT3 '1=>act on toe switch bump sensors fB_EyesWOn VAR Flags.BIT4 '1=>enable eye behavior, 1=>do paceturn on light; default is Eyes Off fB_EyesWalkL VAR Flags.BIT5 '1=>seek light, 0=>seek dark, do paceturn on light fm_TurnedR VAR Flags.BIT6 '0=>last turn=Left, 1=>last turn=Right fs_progspeed VAR Flags.BIT7 '0=>set speed manually, 1=>program sets speed Flags2 VAR Byte f_IRcmnds VAR Flags2.BIT0 '1=>stop executing Acts until IR[0]=9 received ' f2_SA VAR flags.BIT1 'TalkAllophone = inline Allophones f_AutoRun VAR Flags2.BIT2 potspeed VAR Nib speed VAR Byte rollSpeed VAR speed.HIGHNIB 'Servo Speeds paceSpeed VAR speed.LOWNIB 'Servo Speeds splaySpeed VAR speed.LOWNIB 'Servo Speeds hurts VAR Byte hurtsL VAR hurts.HIGHNIB hurtsR VAR hurts.LOWNIB localtemp VAR Byte eyesLR VAR localtemp eyeL VAR eyesLR.HIGHNIB eyeR VAR eyesLR.LOWNIB whiskers VAR Byte '#1 'Twinkletoe VAR whiskers '#1 Eiris VAR Nib '#0 page VAR Nib Aslot VAR Nib minpacespeed CON 1 'for auto decrement S minrollspeed CON 1 'for auto decrement S 'tweenrollpulse CON 16'15'20 16 is about same servo speed as with PicAxeActor clock 'tweenSpacepulse CON 10'15'20 ~5 less for Serin/Serout 'tweenpacepulseplus CON 6 'compensate for no Serial messages 'tweenpulse CON 25'15'20 'Nibmax15 CON 15 'max value for a nibble T2400 CON 396 'PicAxe40, idle high 'N2400 CON 16780 'PicAxe40 idle low N9600 CON 16468 'Toes PicAxe08m2, idle low '-------- Slow variables in CRAM 'LfF,RfB,RfF,LfB as CRAM save two bytes of RAM but add ~12 bytes of program. Setup by 'mrp' RfB CON 0 'same as LfF LfF CON 0 'paceC 'default = no step RfF CON 1 'same as LfB LfB CON 1 'paceC 'default = no step rollL CON 2 'rollC 'default = no roll rollR CON 3 'rollC 'default = no roll 'temp storage 'CRAM_pD CON 4 'temp storage while speaking 'CRAM_pDa CON 5 'temp storage while speaking CRAM_pDsub CON 6 'holds pD during a SubAct 'CRAM_defaultAct CON 7 'CRAM_flagsS CON 8 'temp storage while speaking 'CRAM_pDS CON 9 'temp storage while speaking 'CRAM_pDaS CON 10 'temp storage while speaking CRAM_Apage CON 11 CRAM_Bpage CON 12 CRAM_Aslot CON 13 '**************************** TOM ******************************* 'Smaxtoroll CON 190 'KT ServoMax say +-400us =>1900 'Smintoroll CON 110 'KT ServoMax say +-400us =>1100 'rollC CON 156 'KT roll Centre =upright - best value for turns 'Smaxtopace CON 200 'KT ServoMax 2000uS 'Smintopace CON 100 'KT ServoMax 1000uS 'paceC CON 150 'KT feet together 'SplayC CON 120 'KT feet parallel 'TurnTurnby CON 10 'KT 0-15 for nibble 100uS units 10=>+100 'Splayout CON 240 'KT 2400 +120 'Splayin CON 60 'KT 600 -60 'beeptime CON 110 'K 'beepfreq CON 1900 'K 'beeptime CON 120 ' T 'beepfreq CON 1800 ' T 'beeptime con 100 'POP MOM Joe 'beepfreq con 1800 'POP 'beepfreq CON 2200 'MOM Joe 'rollmin CON 20 'KT 15 min, 30 max (10us), adjust for best walking, 'BTurn DATA m,$76 => roll +-(7+20), pace +-(6*30=210) 'BWalk DATA m,$7A => roll +-(7+20), pace +-(A*30=300) 'pacerollspeedratio CON 1 'KT 2 hurtingalot CON 11 ' T hurting CON 7 ' T hurtalittle CON 3 ' T 'ahurt CON 6 ' T impulse if bumped 'hurtdecay CON 2 'KT 'hurtalittlemark CON hurtalittle -1 'KT for hurts debug 'hurtingmark CON hurting -1 -hurtalittle 'KT for hurts debug 'hurtingalotmark CON hurtingalot -1 -hurting 'KT for hurts debug 'eWgain CON 3 'Joe constant '**************************************************************** 'user commands - ideas 'rt(byte) right turn ->byte-(byte MIN turnpace) do until 0 'lt(byte) left turn ->byte-(byte MIN turnpace) do until 0 't(byte) turn-length =turnpace, works like 's' 'p(0-255) pace-length =strollpace, works like 's' '**************************************************************** '---------------- Robot Commands ---------- 'Action cmnds Z CON $00 'end of list, null move F CON $01 'Forward B CON $02 'Back R CON $03 'Right turn L CON $04 'Left turn H CON $05 'Halt, stand to attention V CON $06 'Voice, beep 1KHz for 0.1sec Br CON $07 'Back with right foot, for toe bump Bl CON $08 'Back with left foot, for toe bump fT CON $09 'footTurn, turn away from leading Foot mT CON $0A 'memoryTurn, turn same way as last time, use f_TurnedR oT CON $0B 'otherTurn, turn other way and toggle f_TurnedR Vt CON $0C 'Voice Tune Vb CON $0D 'Voice Beep 'Behavioural cmnds Su CON $10 'increase speed(+1) MAX potspeed Sd CON $11 'reduce speed(/2) MIN minpacespeed StbOn CON $12 'SensorToeBump on StbOff CON $13 'SensorToeBump off 'frustUp CON 25 'frustration, inc Nibble frust Wlight CON $14 'walk towards light Wdark CON $15 'walk towards light Ewoff CON $16 'Eye Walk off ttyOn CON $17 'debug text On ttyOff CON $18 'debug text Off Ew CON $19 'Eye walk, test and set paceTurn h0 CON $1A 'reduce hurts to zero, otherwise frustration behaviour gets subsumed after next move UStimid CON $1B 'us range 4,6 USnormal CON $1C 'us range 2,4 USbold CON $1D 'us range 1,3 'Parameter cmnds w CON $20 'Wait 1/10 secs in next byte a CON $21 'jump to , in next two bytes s CON $22 'set speeds to next byte, $rp '$55 is max for servos to follow pulse 'from Pot(^15) works best with pacerollspeedratio =1 mrp CON $23 'set Roll and Pace for a Step - to next byte $rollby,stepby 'S03 max speed is 45.5us/frame @20ms/frame, max potspeed is 15 'rollby <$0-$F> delta roll from rollmin 'actualrollby =rollby +rollmin ie rollby=0 => rollC +-rollmin 'if rollby 7 '=>+-(7+15)*10us=*-220us '@ speed 5(10us)=50us '=>440/50*20ms=176ms/STEP & 9 frames 'stepby <$0-$F> '(in 30us units, 15=>450us so pace is 2*450=900us) 'pacepulse =+-(3*stepby)+paceC 'ie 150+-45 => 105 - 195 (10us) => 1050 - 1950us 'if stepby 10 ($A) '=>+-300us '@ speed 5(10us)=50us '=>600/50*20ms=240ms/step & 12 frames 'so at speed=5 rollby=7 stepby=10 one step takes 240+176=416ms 'rolltime/steptime =176/240 =0.73 d CON $24 'set default Act to next byte usa CON $25 'set USaware to next low.nib resolution 2"=>5cm, max 15=>30" usn CON $26 'set USnear to next low.nib resolution 2" T CON $27 'Talk #msg in next byte numbers 0,1,2,3,4,5,6,7,8,9 (10,11,12,13,14,15) and St CON "t" 'Test - my name_is condor wildeagle sam efi tom joe Sname CON "n" 'Name - hello I am kasper ''Sact CON "a" ''Sforward CON "f" ''Sback CON "b" ''Sright CON "r" ''Sleft CON "l" ''Sobstacle CON "o" BASIC_Stamp CON "s" 'Hello I am the BASIC Stamp SubAct CON $28 'do Sub-act in next byte, must be in the same page tty CON $29 'debug text starting in next byte and ending with ¬ Xacts CON $2A 'dump acts then behaves to terminal, only from IR 'parameter test cmnds usIFa CON $2B 'US if =< Aware ; if USrange =< usAwareAt then do Act in next byte usIFn CON $2C 'US if =< Near ; if USrange =< usTooNear then do Act in next byte usIFm CON $2D 'US if < mem ; if USRange < USlast do Act in next byte usIFL CON $2E 'US if Less ; if USRange in next byte 'Hurt rule cmnds IFhLR CON $40 'Do ToesRL hurt IFhR CON $41 'Do Toe R hurts IFhL CON $42 'Do Toe L hurts 'Parameters '---------------- Acts ------------------- 'if not ended with 'z' continue with next Act 'Acts can have any legal variable name. 'first Act DATA must be @>0 since Z=> pD=0 means nothing to do DATA "A" 'ASCII 65 - not a command Ainit DATA Vt,Vb,V,mrp,$77,ttyOn,H,T,Sname,w,5,V,V,Z 'initialisation Act done after PowerOn or Reset A15 DATA V,Z 'just so we know the switch is on -1 not 0 !!! A14 DATA A13 DATA A12 DATA A11 DATA A10 DATA A9 DATA 'A8 DATA tty,CR,"'8'¬",slot,4,c,7,V,V,V,V,V,V 'A8 DATA tty,"test¬",SubAct,8,tty,"done¬",a,A8 A8 DATA tty,CR,"'8' doing a sub",CR,"¬",z A7 DATA T,7,T,"n",w,20,tty,"going bac¬",slot,3,c,7 'A7 DATA tty,CR,"'7'¬",SubAct,A8,tty,"--back from sub",CR,"¬",a,0,A7 'A7 DATA tty,CR,"'7'¬",usIFL,10,A8,a,0,A7 A6 DATA tty,"A6¬",T,6,z A5 DATA '"A5",tty,CR,"5¬",T,5,slot,3,c,7 A4 DATA '"A4",tty,CR,"4¬",T,4,slot,3,c,7 A3 DATA '"A3",tty,CR,"3¬",T,3,T,"n",slot,3,c,7 A2 DATA '"A2",tty,CR,"2¬",T,2,T,"n",slot,3,c,7 'A1 DATA tty,CR,"1¬",T,1,T,"n",tty,"talked¬",slot,3,3,c,7 A1 DATA tty,"A1¬",T,1,subact,A6,T,Sname,tty,"talked¬",slot,3,c,7 A0 DATA tty,"A0¬",T,0,T,"n",slot,3,c,7 Aend DATA Z 'end marker to avoid doing the program! endA DATA 255 'end marker for Dump 'Acts slot=4 page=0 ' 00 01 02 03 04 05 06 07 08 09 0A 0B 0C 0D 0E 0F '00 - 41,0C,0D,06,23,77,17,05,27,6E,20,05,06,06,00,06, '10 - 00,29,0D,27,38,27,20,64,6F,69,6E,67,20,61,20,73, '20 - 75,62,0D,AC,00,27,07,27,6E,20,14,29,67,6F,69,6E, '30 - 67,20,62,61,63,AC,30,03,2F,07,29,41,36,AC,27,06, '40 - 00,29,41,31,AC,27,01,28,3A,27,6E,29,74,61,6C,6B, '50 - 65,64,AC,30,03,2F,07,29,41,30,AC,27,00,27,6E,30, '60 - 03,2F,07,00,FF, '---------------------------------------------- DATA @256 DATA "B" 'ASCII 66 - not a command 'first Act DATA must be @>0 since Z=> pD=0 means nothing to do '---------------- Tactile behavioural rules --- HurtRules DATA tty,"Hrules¬" frus2 DATA IFhLR,hurtingalot,Af2 'L+R >hurtingalot frus1 DATA IFhLR,hurting,Af1 'L+R >hurting hurtR DATA IFhR,hurtalittle,AbR 'R >ahurt hurtL DATA IFhL,hurtalittle,AbL,Z 'L >ahurt '---------------- frustration behavioural Acts ------- 'inhibit sensors until Toe moved back a bit, otherwise just loops to here Af1 DATA tty,"Af1R¬",h0,StbOff,V,V,V,B,StbOn,mT,mT,Z 'if (hurtsR+hurtsL)>hurting Af2 DATA tty,"Af2L¬",h0,StbOff,V,V,V,V,V,B,StbOn,B,mT,mT,mT,Z 'if (hurtsR+hurtsL)>hurtalot '---------------- bump behavioural Acts ------------- 'if toeX bumped then hurtsX += ahurt 'inhibit sensors until Toe moved back a bit, otherwise just loops to here AbR DATA tty,"AbR¬",StbOff,V,Sd,Br,h0,StbOn,L,L,Z 'if hurtsR>hurtalittle AbL DATA tty,"AbL¬",StbOff,V,V,Sd,Bl,h0,StbOn,R,R,Z 'if hurtsL>hurtalittle '---------------- US behavioural Acts ------------- USTfScan DATA tty,"`USTfScan¬" DATA V,fT,usIFm,USoTscan,usIFa,USmT,Z 'TurnScan, using fT means centre doesn't move (much) USoTscan DATA tty,"`USoTscan¬" 'DATA V,V,oT,bk,mT,mT,usIFL,4,USmT,Z 'OtherTurnScan bk only used for skid turns DATA V,V,oT,mT,mT,usIFa,USmT,Z 'OtherTurnScan USmT DATA tty,"`USmT¬" 'DATA V,V,V,bk,mT,mT,usIFL,4,USmT,Z bk only used for skid turns DATA V,V,V,mT,mT,usIFa,USmT,Z 'USslow DATA Sd,Z USback DATA B,fT,B 'fall through and test again '---------------- US behavioural Rules ------------- 'Behaviour variable thresholds and BehaviouralAction 'US: ActionRange,Act USavoid DATA V,tty," USavoid¬" ' DATA frustUp 'UScaution DATA tty,"caut¬",usIFL,3,USslow UStooclose DATA tty," close¬",usIFn,USback 'Joe 3 USinway DATA tty," inway¬",usIFa,USTfScan DATA Z '---------------------------------------------- Bend DATA Z 'end marker to avoid doing the program! endB DATA 255 'end marker for Dump '---------------------------------------------- 'BActs slot=4 page=1 ' 00 01 02 03 04 05 06 07 08 09 0A 0B 0C 0D 0E 0F '00 - 42,29,48,72,75,6C,65,73,AC,40,0B,26,40,07,16,41, '10 - 03,3A,42,03,48,00,29,41,66,31,52,AC,1A,13,06,06, '20 - 06,02,12,0A,0A,00,29,41,66,32,4C,AC,1A,13,06,06, '30 - 06,06,06,02,12,02,0A,0A,0A,00,29,41,62,52,AC,13, '40 - 06,11,07,1A,12,04,04,00,29,41,62,4C,AC,13,06,06, '50 - 11,08,1A,12,03,03,00,29,60,55,53,54,66,53,63,61, '60 - 6E,AC,06,09,2D,69,2B,7C,00,29,60,55,53,6F,54,73, '70 - 63,61,6E,AC,06,06,0B,0A,0A,2B,7C,00,29,60,55,53, '80 - 6D,54,AC,06,06,06,0A,0A,2B,7C,00,02,09,02,06,29, '90 - 20,55,53,61,76,6F,69,64,AC,29,20,63,6C,6F,73,65, 'A0 - AC,2C,8B,29,20,69,6E,77,61,79,AC,2B,57,00,00,FF, '---------------------------------------------- 'System Commands 'Robot cmnds for Run0 passed in cmnd 'cmnds for Run1 'Lroll CON 0 'Croll CON 1 'Rroll CON 2 'cmnds for Run2 '"tnfbrlso" ' cmnd=255 ON RETURN from speech 'cmnds for Run3 'Act/cmnd 0-16 DoHurtRules CON $40 '---------------------------------------------- init: IF cmnd=Xacts THEN dumpActs IF cmnd=255 THEN START 'no valid cmnd 'newAct: DEBUG"syscmnd=",HEX cmnd DEBUG" -40" LOOKUP cmnd -$40,[HurtRules],pDb '#1 IF pDb<>0 THEN page =1 :PUT CRAM_Bpage,page :RUN 0 :ENDIF'new Behave DEBUG" -2B" BRANCH cmnd -$2B,[_usIFa,_usIFn,_usIFm,_usIFL] 'need old pD DEBUG" -29" BRANCH cmnd -$29,[_tty] DEBUG" -21" BRANCH cmnd -$21,[doNewAct] '"a" needs old pD DEBUG" -00" page =0 :PUT CRAM_Apage,page LOOKUP cmnd,[A0,A1,A2,A3,A4,A5,A6,A7,A8,A9,A10,A11,A12,A13,A14,A15,Ainit],pD IF pD<>0 THEN page =0 :GOTO START :ENDIF DEBUG" illegal",CR cmnd =0 'LEAVES ---------------------------------------- Run0: RUN 0 '------------------------------------------------ doNewAct: READ 256 *page +pD ,pD page =param GOTO START '------------------------------------------------ incNewAct: '_IFhLR,_IFhL,_IFhR pD =pD +1 newAct: '"a" 'never change here IF pDa=0 THEN pDa =pD 'save pD if changing to behave DEBUG HEX pD READ 256 *page +pD ,pD DEBUG" newAct@",HEX pD',CR '------------------------------------------------ START: DEBUG CR," S" '******************************************* QearCmnd: IF EarRdy_=0 THEN f_IRcmnds =1 HIGH BSeRdy SERIN EarListen,N9600,2000,Run0,[cmnd,param] 'idle is LOW 'DEBUG"IR-$",HEX cmnd,"(",DEC cmnd,"),",HEX param,"(",DEC param,")",CR LOW BSeRdy IF cmnd=9 THEN F_IRcmnds =0 :f_AutoRun =1 :cmnd =Z :ENDIF 'IRkey[0]=9 IF cmnd=Xacts THEN dumpActs IF cmnd=c THEN F_IRcmnds =0 :f_AutoRun =0 :cmnd =param :GOTO init :ENDIF 'no autorun if IR IF cmnd=H THEN pDa =0 :pDb =0 :PUT CRAM_pDsub,0 :Aslot =3 RUN 0 'do cmnd ENDIF IF F_IRcmnds THEN PulseServos 'and GOTO START Read_cmnd: READ 256 *page +pD ,cmnd READ 256 *page +pD +1 ,param DEBUG "@",DEC page,",",HEX pD',"(",DEC pD,")c",HEX cmnd,"(",DEC cmnd,")" pD =pD +1 IF cmnd=tty THEN _tty IF cmnd>=IFhLR THEN IFhurt 'below RUN 0 '--------------------- _tty: IF param="¬" THEN pD =pD +1 : GOTO START IF fB_tty=1 THEN DEBUG param pD =pD +1 'point to next parameter READ 256 *page +pD,param GOTO _tty '--------------------- '2 Parameter cmnds '60- IFhurt: BRANCH cmnd -IFhLR,[_IFhLR,_IFhR,_IFhL] GOTO START '--------------------- 'Tactile_rules: 'already in pageB _IFhLR: DEBUG"hLR" IF param > (hurtsR+hurtsL) THEN pDinc2START 'skip Bact to do GOTO incNewAct _IFhR: DEBUG"hR" IF param > hurtsR THEN pDinc2START 'skip Bact to do GOTO incNewAct _IFhL: DEBUG"hL" IF param > hurtsL THEN pDinc2START 'skip Bact to do GOTO incNewAct pDinc2START: pD =pD +2 GOTO START '--------------------- _usIFa: DEBUG" usIFa pD",HEX pD',",",HEX param IF USRange<=USawareAt THEN jumpParam GOTO START _usIFn: DEBUG" usIFn pD",HEX pD',",",HEX param IF USRange<=UStooNear THEN jumpParam GOTO START _usIFm: DEBUG" usIFm pD",HEX pD',",",HEX param IF USRange<=USmem THEN jumpParam GOTO START jumpParam: pDb =param RUN 0 '--------------------- _usIFL: ' - needs work? DEBUG" usIFL pD",HEX pD',",",HEX param,"<",DEC USRange,CR IF USRange<=param THEN newAct pD =pD +1 GOTO START '----------------------------------------------------------------------------------------- PulseServos: PULSOUT servoroll,rollRat*5 'rollRat*5 '10uS to 2uS for BS2 PULSOUT servopace,paceLat*5 'paceLat*5 '10uS to 2uS for BS2 PULSOUT servosplay,splayat*5 'splayat*5 '10uS to 2uS for BS2 PAUSE 20 GOTO START '------------------------------------------------------------------ dumpActs: DEBUG CR pageBase VAR i page =0 DEBUG"'Acts " GOSUB _Xacts page =1 DEBUG CR,"'BActs " GOSUB _Xacts STOP '*****************STOP _Xacts: DEBUG "slot=",DEC Aslot," page=",DEC page,CR,"' " pageBase =page *256 FOR cmnd=0 TO 15 DEBUG HEX2 cmnd," " NEXT 'cmnd DEBUG CR cmnd =0 DO DEBUG "'",HEX2 cmnd *16," - " FOR param=0 TO 15 READ cmnd *16 +param +pageBase ,turnby DEBUG HEX2 turnby,"," IF turnby=255 THEN EXIT NEXT 'param DEBUG CR IF turnby=255 THEN EXIT cmnd =cmnd+1 LOOP RETURN '------------------------ end program ------------------- '------------------------------------------------------------------