'{$STAMP BS2e} '{$PBASIC 2.5} 'Kas14-1.BSe DEBUG CR,"{141,$",HEX cmnd,"}s",DEC speed 'prog0 'prog1 move 'prog2 talk 'prog3 DATA '------------------------------------------------------------------ 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 'PicAxe40x 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 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: ' fB_Bdir VAR flags.BIT1 '1=>Bk 0=>halt 'for _H: ' 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 Twinkletoe VAR whiskers 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 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 '**************************** KAS ******************************* 'Pace servo S03 +ve anti-clock 'Roll servo S03 +ve anti-clock 'Turn servo S03 +ve anti-clock 'Smaxtoroll CON 190 'KT ServoMax say +-400us =>1900 'Smintoroll CON 110 'KT ServoMax say +-400us =>1100 rollC CON 152 'K roll Centre =upright - best value for turns 'Smaxtopace CON 200 'KT ServoMax 2000uS 'Smintopace CON 100 'KT ServoMax 1000uS paceC CON 152 '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 15 'K hurting CON 10 'K hurtalittle CON 7 'K ahurt CON 9 'K 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 '**************************************************************** 'System Commands '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: ON cmnd GOTO L_roll,C_roll,R_roll 'LEAVES ---------------------------------------- cmnd255RunAslot: cmnd =255 'cmnd=255 => no cmnd RUN Aslot '******************************************* 'RollSet: '[roll servo dependent] L_roll: 'from - F, B, H GET rollL,rollRto splayto =turnbyR *10 +splayC 'turnby 100uS units -> 10us turnby =0 GOTO moveservos C_roll: 'from - R, L rollRto =rollC GOTO moveservos R_roll: 'from - F, B, H GET rollR,rollRto splayto =turnbyL *10 +splayC 'turnby 100uS units -> 10us turnby =0 'GOTO moveservos moveservos: 'move servos in units of 10us DEBUG"m" 'DEBUG ? splayto 'IF fs_servosOff THEN START '--------roll onto foot FORr: DEBUG"r" FOR rollRat =rollRat TO rollRto STEP rollspeed 'DEBUG ?rollRat GOSUB PulseServos PAUSE tweenrollpulse NEXT rollRat =rollRto 'note - FOR increments rollRat past rollRto '--------move other foot FORps: DEBUG"p" FOR paceLat =paceLat TO paceLto STEP pacespeed 'DEBUG ?paceLat,DEC paceLto IF splayto>=splayat THEN splayat =pacespeed *2 +splayat MAX splayto ELSE splayat =pacespeed *-2 +splayat MIN splayto ENDIF GOSUB PulseServos IF fB_StOn=0 THEN PAUSEpace SEROUT 0,N9600,["B"] SERIN 0,N9600,50,p_notoe,[Twinkletoe] LOW 1 IF Twinkletoe>0 THEN S_Rhurt 'abort move, after Rhurt is calculated L Ttoe will be tested SEROUT 1,N9600,["B"] SERIN 1,N9600,50,p_notoe,[Twinkletoe] IF Twinkletoe>0 THEN S_Lhurt 'abort move, since R Ttoe was 0 just goto S_Lhurt: GOTO PAUSESpace p_notoe: DEBUG"xB" ' IF fB_StOn AND NOT(S_toeL AND S_toeR) THEN quitmove 'and abort move, toe switches active LOW PAUSEpace: PAUSE tweenpacepulseplus PAUSESpace: PAUSE tweenSpacepulse NEXT paceLat =paceLto 'note - FOR increments paceLat past rollRto '''--------splay do remainder of splay move, if any FORs: DEBUG"s" IF splayto<>splayat THEN temp =pacespeed *2 FOR splayat =splayat TO splayto STEP temp GOSUB PulseServos NEXT splayat =splayto ENDIF '--------roll upright rollRto =rollC FORu: DEBUG"u" FOR rollRat =rollRat TO rollRto STEP rollspeed GOSUB PulseServos PAUSE tweenrollpulse NEXT rollRat =rollRto 'note - FOR increments rollRat past rollRto GOSUB PulseServos 'make sure servo is at rollRto quitmove: IF fB_StOn=0 THEN cmnd255RunAslot 'ignore touch sensors 'DEBUG "endmove" 'CalcHurts: 'S_R: ' IF S_toeR=1 THEN S_L SEROUT S_toeR,N9600,["T"] SERIN S_toeR,N9600,50,h_notoe,[Twinkletoe] IF Twinkletoe=0 THEN S_L S_Rhurt: hurtsR =hurtsR +ahurt MAX Nibmax15 FREQOUT voicepin,100,1500 S_L: ' IF S_toeL=1 THEN Qhappy SEROUT S_toeL,N9600,["T"] SERIN S_toeL,N9600,50,h_notoe,[Twinkletoe] IF Twinkletoe=0 THEN Qhappy S_Lhurt: hurtsL =hurtsL +ahurt MAX Nibmax15 FREQOUT voicepin,100,1500 Qhappy: 'DEBUG "hurts",DEC hurtsL,DEC hurtsR,CR IF (hurtsL +hurtsR)=0 THEN cmnd255RunAslot 'ShowHurts: DEBUG CR,"hurtsLR",DEC hurtsL,";",DEC hurtsR 'DEBUG CR,REP" "\hurtalittlemark,"|",REP" "\hurtingmark,"|",REP" "\hurtingalotmark,"|" DEBUG CR,REP" "\hurtalittlemark,HEX1 hurtalittle,REP" "\hurtingmark,HEX1 hurting,REP" "\hurtingalotmark,HEX1 hurtingalot DEBUG CR,REP"L"\hurtsL DEBUG CR,REP"R"\hurtsR DEBUG CR,REP"+"\hurtsR+hurtsL,CR morehappy: 'if this is after START then decays each rule evaluation and decays too fast for params ' so rules never trigger!!!! hurtsR =hurtsR MIN hurtdecay -hurtdecay hurtsL =hurtsL MIN hurtdecay -hurtdecay cmnd =DoHurtRules '21 system command RUN Aslot '------------------ h_notoe: 'here if no reply from Toes DEBUG"xhT" GOTO morehappy '--------------------------------------------- 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 RETURN '------------------------ end program ------------------- '------------------------------------------------------------------