'{$STAMP BS2} ' {$PBASIC 2.5} DEBUG CLS,"Joe5-5.bs2",CR 'Do '-- 'Bugs '---- ' doA: ok so what is wrong???? Sep2010 '------------------------------------------------------------------ 'Notes '----- 'BS2, BS2e EEPROM 10million writes - put stack in EEPROM 'BS2sx, BS2p EEPROM 100thousand writes - put stack in commonRAM ' 3,600 seconds/hr ' 86,400 seconds/day '31,536,000 seconds/year 'say max working life of an Ambler is 40hrs a week for 13 weeks =>1,872,000 seconds '-------------- '5v supply is from 7805 regulator '-------------- '>JoeActor-PicAxe18x.bas see at end 'runs from 5v 'displays default Act and SERs [255,Act,wiskers,eyesLR] every ~21.5ms, 'Act read in init: 'eyesRL read in _Ew: '-------------- 'Ping US Sensor runs off 5v '-------------- 'putting Hurt rules in DATA means they are slower to evaluate and there is a pause between ' each step if (hurtR+hurtL)<>0 '------------------------------------------------------------------ 'History '------- '26Nov19 1CC changed pot in Pace servo (old one bad) pace centre 144 '28Aug17 1CD added Twinkletoes 'Joe5.5.BS2 '01Jun15 245 removed fs_progspeed =0 from _su,_sd '28May15 xxx corrected position of tbOn in Af1, Af2 (was before B) ' StbOn,StbOff changed TO tbOn,tbOff ' reorganised route to US walk, added IFus and US flag fB_USon ' usIfL -> IFusL, usIfLm -> IFusLm 'Joe5.4.BS2 '18Jul11 242 added _Ewoff, AND changed fB_EyesOn to fB_EyesWOn ' changed _eW: to avoid byte rollover '26Jun11 249 corrected hurtR/L: to use hurtalittle not ahurt so they get used '26Jun11 249 added h0, rearranged flags '08Oct10 262 removed serin servo clock from PicAxe, used PAUSE tweenpulse as Mom '26Sep10 241 changed usdIF,usaIF to usIF,,. usd deleted, usa not used yet '23Sep10 Qhappy: THEN morehappy -> THEN START, pointless going to morehappy 'Joe5.3.BS2 - put hurt rules in DATA '09Jan09 1F4 got Ew working, brought Joe code in line with Mom code '09Jan09 added eyes '02Nov08 356 got PicAxe18 code working, tweenpulse of 20 =>~28+ms frame rate, s1 is better '30Oct08 added PicAxe18Actor and got code working '27Oct08 Joe5.2 from Mom5.2, changed servo pins '27Oct08 added preturn for guided turning and walking '26Oct08 36D added Br,Bl and removed backbump and f_bump, now the foot that gets bumped gets moved '24Oct08 364 34C added PicAxe08Actor to set default Act, f_Sbdo changed to f_StOn '23Oct08 3B3, added usm,Baturn '22Oct08 3FC, added SRF05 ultrasonic sensor, T,OT,us,usr,Bh,USscan1,USscan2,USavoid '18Oct08 Changed Stride back to Pace (why did this seem to save a lot of bytes?) ' made Su,Sd instead of s,0 s,255 ' took Bump tests out of servo loop and reduced CONs, they overflowed bytes anyway ' Mom7 has logical Roll and Pace so RollR and PaceR are +ve ' Mom5.1 has servo Roll and Pace so RollR is +ve and PaceL is +ve ' had to put in RollUpright in Servo move because action wasn't smooth without it '17Oct08 Got rid of Act Beh cmnds, bump entry is now START not CMNDCASE 'MOM5.1.BS2 '[14-6-04 TOM5, BS2E, 2us resolution, pages not used so no code changes,] '[23-5-04 JOE5, pins altered for Ambler production pcb ] '29-5-03 direction of roll changed in _H to prevent double R roll in init, now L,R '25-5-03 jmp removed and A added, Da changed to Ax for Acts, Acts can now be linked 'MOM5 Df2 added ' two bump behaviours, two frustration behaviours 'MOM4 tests put in servo loop, means all limits have to be a lot bigger 'MOM3 bSense now only activates a flinch, controlled by an expectancy flag ' this works but tests slow things down in between cmnds ' lesshappy now in bSense and incremented every toe bump sense, ' still only once before jumping to Bbehav, 'MOM2 lesshappy was in _Read but now in bSense so values are different ' no problem with stackmove being lost. ' maxhurts, hurtsplus, hurtsminus added. ' lesshappy depended on position of behavior being after Db ' [if pD>Db then lesshappy] which I didn't think was a good thing, ' won't work if read from a disk say. ' two bump behaviours, one frustration behaviour '8-9-02 mood added '26-8-02 Stride was 1/2 stride about strideC, now stride is real stride from ' tostride =strideC +-stride to ' tostride =atstride +-stride Max/Min maxtostride/mintostride. ' Easier to alter stride for turns and turn while walking ' Stride now a var, added ~12bytes of pgm. ' Turnstride added. ' in _S turnstride =strollstride-stridespeed ' - doesn't help much in corners '24-8-02 now working properly '23-8-02 Do list now terminated with z, bump lists added ' Pace changed to Stride, less ambiguous ' various names changed '21-8-02 turnbias removed because with MOM 0 is best ' in move ForNext loop used - less code ' both servos pulsed, allows servos to catch up to pulse values ' both servos can be seen to move when fast '20-8-02 pins, servo directions and values changed for MOM ' tests rfootat>p_stand changed round to suit new servo values ' was rfootat>p_stand => rfoot fd, now rfoot=bk>p_stand 'pE now pD 'BigFt-1S.bas for Stamp1 converted to BS2 'based on BigFt-1S.bas 'based on BigFt-1 '_WAIT added, _Stop removed, jump changed to _Start, labels changed 'alias rfootat=atpace added 'pE was pu 'Sensors incorporated 'stackmove added 'nomove added '------------------------------------------------------------------ 'SuperTec servos '--------------- '0.23sec/60deg 3.4Kgcm 'as measured 2110us - 540us for 180deg => 1570us/180deg '=>523us/60deg @ 20ms/frame 0.23sec/60deg =>11.5frames/60deg =>523/11.5 us/frame = 45.5us/frame max ' For pace of 900us @10us resolution, s=1 => 10us/frame =>90 frames => 1.8s ' For pace of 900us @10us resolution, s=5 => 50us/frame => 18 frames => 0.36s ' For pace of 900us @10us resolution, s=10 => 100us/frame => 9 frames => 0.18s FASTER THAN SERVO!!! 'speed can't be faster then 5 for full servo travel 'pulse values in 10us units, ie 100=1ms, 200=2ms 'roll - 100=left 150=stand 200=right nominal 'stride - 200=left(FD) 150=stand 100=right(FD) nominal 'Other info '---------- 'Pot=10k + 0u1 =>533 '------------------------------------------------------------------ ' Joe Board ' FRONT '_______________________________________ '| P15 95 data 0M P5 | bus connector - 9v 5v P15 P14 ....P9 P8 0v MR '| Aux15 vv76543210vR Aux5a | '| ooo oooooooooooo ooo | '| 0v 5v p15 p5 5v 0v | '| | '| ooo | p5 5v 0v - Ping '| | '| ooo | p4 5v 0v - PicAxe Actor/EyeL/EyeR/whiskers '| | '| ooo | p3 6v 0v - Servo Pace '| foot cables | '| B3 0v 5v P0 P1 B6 ooo | p2 6v 0v - Servo Roll '| B0 0v 5v P0 P1 B3 | '| Bx - servo battery | '| D9 RS232 | Stamp programming connector and debug at 9600 baud '|_____________________________________| '------------------------------------------------------------------ S_toeR_ VAR IN0 'right toe switch on d0 - normally high bump low 'PIN 0 SEROUT["T"],SERIN[Ttoe] - Ttoe=0 no obstacle S_toeL_ VAR IN1 'left toe switch on d1 - normally high bump low 'PIN 1 SEROUT["T"],SERIN[Ttoe] - Ttoe=0 no obstacle servoroll CON 2 'servo on pin 2 'MOM 4 servopace CON 3 'servo on pin 3 'MOM 5 'servoAux4 CON 4 'Auxservo on pin 4 has 6v to powerpin PicAxe18Actor PIN 4 'serin from PicAxe18Actor, uses servoAux4(ie 6v) 'servoAux5 CON 5 'Auxservo on pin 5 has 6v to powerpin SRF05 PIN 5 'pin for both trigger and echo, uses servoAux5(ie 6v) 'LeftAux4a CON 4 'LeftAux on pin 4 has 5v to powerpin 'RightAux5a CON 5 'RightAux on pin 5 has 5v to powerpin voicepin CON 6 potpin CON 7 'user pot on pin 7 'PIN 08 'MOM PicAxe08Actor PIN 8 'serin from PicAxe08Actor 'PIN 09 'PIN 10 'PIN 11 'PIN 12 'PIN 13 'PIN 14 START: OUT14 =fB_EyesWalkL 'PIN 15 'Mom SRF05 PIN 15 '-------------------------- rollRat VAR Byte rollRto VAR Byte 'rollC var byte 'used for **determine rollC** section paceLat VAR Byte paceLto VAR Byte pD VAR Word 'pointer to current move in DATA pDa VAR Byte 'pointer to current move in Acts=aDATA pDb VAR Word 'pointer to selected behaviour in bDATA cmnd VAR Byte 'current command Act VAR cmnd 'init:, servo routine SERIN param VAR Byte 'current command parameter temp VAR param 'init:, servo routine tempD VAR param 'local to _d: stackcmnd VAR Byte 'for a pending move potval VAR Word 'readpot: getspeed: i VAR potval 'FOR counter ' eyeRCtime VAR potval 'Readeyes: USrange VAR potval 'US raw range value maxval VAR potval.HIGHBYTE 'move: minval VAR potval.LOWBYTE 'move: Ltemp VAR potval.HIGHBYTE 'Ew Rtemp VAR potval.LOWBYTE 'Ew USAwareAt VAR Nib 'US Aware Range resolution 2"=>5cm, max 15=>30" 'USDoAt VAR Nib 'US Action Range resolution 2"=>5cm, max 15=>30" USmem VAR Nib 'US previous Range resolution 2"=>5cm, max 15=>30" Flags VAR Byte fB_Fdir VAR flags.BIT0 '1=>Fd 0=>Bk 'for _H: and StepandTurn: fB_tty VAR flags.BIT1 'tty control, 1=>print, 0=>no printing fB_StOn VAR flags.BIT2 '1=>act on toe switch bump sensors fB_EyesWOn VAR flags.BIT3 '1=>enable eye behavior, 1=>do paceturn on light; default is Eyes Off fB_EyesWalkL VAR flags.BIT4 '1=>seek light, 0=>seek dark, do paceturn on light fm_TurnedR VAR flags.BIT5 '0=>last turn=Left, 1=>last turn=Right fs_progspeed VAR flags.BIT6 '0=>set speed manually, 1=>program sets speed fB_USon VAR flags.BIT7 '1=> do US potspeed VAR Nib speed VAR Byte rollSpeed VAR speed.HIGHNIB 'Servo Speeds paceSpeed VAR speed.LOWNIB 'Servo Speeds hurts VAR Byte hurtsL VAR hurts.HIGHNIB hurtsR VAR hurts.LOWNIB paceTurn VAR Byte '+ve turn left 'has to be a byte so -ve works in Move: localtemp VAR Byte eyesLR VAR localtemp eyeL VAR eyesLR.HIGHNIB eyeR VAR eyesLR.LOWNIB whiskers VAR Byte Ttoe VAR whiskers 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 Nibmax15 CON 15 'max value for a nibble N4800 CON 16572 'to receive Act from Actor PicAxe18x, idle low N9600 CON 16468 'to receive Act from Toes PicAxe08m2, idle low '-------- Slow variables in EEPROM 'LfF,RfB,RfF,LfB as DATA save two bytes of RAM but add ~12 bytes of program RfB DATA 'same as LfF LfF DATA paceC 'default = no step RfF DATA 'same as LfB LfB DATA paceC 'default = no step rollL DATA rollC 'default = no roll rollR DATA rollC 'default = no roll 'USArange DATA 4 'default value, 'US Action Range 'stackcmnd 'USlast '**************************** JOE ******************************* Smaxtoroll CON 190 'ServoMax MOM POP Joe say +-400us =>1900 Smintoroll CON 110 'ServoMax MOM POP Joe say +-400us =>1100 rollC CON 150 'Joe roll Centre =upright Smaxtopace CON 200 'ServoMax MOM POP Joe 2000uS Smintopace CON 100 'ServoMin MOM POP Joe 1000uS paceC CON 144'150 'feet together beeptime CON 100 'MOM Joe beepfreq CON 2200 'MOM Joe 'beeptime con 100 'POP 'beepfreq con 1800 'POP 'roll 15 min, 30 max (10us), adjust for best walking, rollmin CON 15 'BTurn DATA m,$76 => roll +-(7+15), pace +-(6*30=210) 'BWalk DATA m,$7A => roll +-(7+15), pace +-(A*30=300) pacerollspeedratio CON 1 hurtingalot CON 11 hurting CON 6 hurtalittle CON 3 ahurt CON 6 'impulse if bumped hurtdone CON 1 'reduction when actioned hurtdecay CON 1 hurtalittlemark CON hurtalittle -1 'for hurts debug hurtingmark CON hurting -1 -hurtalittle 'for hurts debug hurtingalotmark CON hurtingalot -1 -hurting 'for hurts debug eWgain CON 1'3 'afrustration CON 100 'frustthresh CON 240 'afrustration CON 80 'frustthresh CON 160 '**************************************************************** 'user routine names - 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' '**************************************************************** '---------------- user commands ---------- 'Action cmnds Z CON 0 'end of list, null move F CON 1 'Forward B CON 2 'Back R CON 3 'Right turn L CON 4 'Left turn H CON 5 'Halt, stand to attention V CON 6 'Voice, beep 1KHz for 0.1sec Br CON 7 'Back with right foot, for toe bump Bl CON 8 'Back with left foot, for toe bump fT CON 9 'footTurn, turn away from leading Foot mT CON 10 'memoryTurn, turn same way as last time, use f_TurnedR oT CON 11 'otherTurn, turn other way and toggle f_TurnedR Vt CON 12 'Voice Tune Vb CON 13 'Voice Beep 'Behavioural cmnds Su CON 20 'increase speed(+1) MAX potspeed Sd CON 21 'reduce speed(/2) MIN minpacespeed tbOn CON 22 'SensorToeBump on tbOff CON 23 'SensorToeBump off bk CON 24 'set fb_Fdir to 0 so StepandTurn is stepback 'frustUp CON 25 'frustration, inc Nibble frust Wlight CON 25 'walk towards light Wdark CON 26 'walk towards dark Ewoff CON 27 'Eye Walk off ttyOn CON 28 'debug text On ttyOff CON 29 'debug text Off Ew CON 30 'Eye walk, test and set paceTurn h0 CON 31 'reduce hurts to zero, otherwise frustration behaviour gets subsumed after next move usOn CON 32 usOff CON 33 'Parameter cmnds 'could use '(ASC 39) for tty comments ,"'comment'" instead of tty,"comment¬" w CON 40 'Wait 1/10 secs in next byte a CON 41 'jump to Act in next byte s CON 42 'set speeds to next byte, $rp '$55 is max for servos to follow pulse 'from Pot(^15) works best with pacerollspeedratio =1 m CON 43 'set Roll and 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 44 'set default Act to next byte usa CON 45 'set USaware range(<=) to next low.nib resolution 2"=>5cm, max 15=>30" IfusOn CON 46 IfusL CON 47 'US if Less ; if USRange; if USRange8" A0 DATA w,1,Z 'just wait '---------------- frustration behavioural Acts ------- 'inhibit sensors until Toe moved back a bit, otherwise just loops to here Af1 DATA tty,"Af1R¬",h0,tbOff,V,V,V,B,tbOn,mT,mT,Z 'if (hurtsR+hurtsL)>hurting Af2 DATA tty,"Af2L¬",h0,tbOff,V,V,V,V,V,B,tbOn,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¬",tbOff,V,Sd,Br,B,tbOn,L,Z 'if hurtsR>hurtalittle AbL DATA tty,"AbL¬",tbOff,V,V,Sd,Bl,B,tbOn,R,Z 'if hurtsL>hurtalittle '---------------- US behavioural Acts ------------- USTfScan DATA tty,"`USTfScan¬" DATA V,fT,IfusLm,USoTscan,IfusL,4,USmT,Z 'TurnScan, using Tf means centre doesn't move (much) USoTscan DATA tty,"`USoTscan¬" DATA V,V,oT,bk,mT,mT,IfusL,4,USmT,Z 'OtherTurnScan bk so so stepturn doesn't move closer USmT DATA tty,"`USmT¬" DATA V,V,V,bk,mT,mT,IfusL,4,USmT,Z 'MemoryTurn bk so so stepturn doesn't move closer 'USslow DATA Sd,Z USback DATA B,fT '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¬",IfusL,2,USback 'Joe 3 USinway DATA tty,"way¬",IfusL,4,USTfScan DATA Z '---------------- 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 '---------------------------------------------- Bend DATA Z 'end marker to avoid doing the program! '---------------------------------------------- init: 'Joe, PicAxe18 specific ' SEROUT 0,N4800,["B"] ' SERIN 0,N4800,[Ttoe] 'DEBUG Ttoe,CR 'GOTO init 'press either toe at poweron to disable servos, this is for testing since Ping, PicAx18 use 6v 'IF S_toeL+S_toeR<>2 THEN fs_servosOff=1 'sensors normally high SERIN PicAxe18Actor,N4800,[Act,Act,Act,Act] 'waste four junk bytes if BS2 switched on before PicAxeActor ' otherwise Act is sometimes wrong. SERIN PicAxe18Actor,N4800,[WAIT(255),Act] 'get cmnd from PicAxe18 DEBUG "Act=",DEC Act LOOKUP Act,[A0,A1,A2,A3,A4,A5,A6,A7,A8,A9,A10,A11,A12,A13,A14,A15],pD 'NewMind: READ defaultAct,temp IF pD<>temp THEN WRITE defaultAct,pD 'only WRITE if different 'nocmnd: rollRat =150 'nominal upright, in case rollC var is used, avoids var=0 paceLat =paceC fs_progspeed =0 fB_Fdir =1 'default direction is Forward pD =Ainit 'set pointer to initialisation act START: OUTPUT 14 OUT14 =fB_EyesWalkL 'debug LED DEBUG CR',">",DEC pD IF pDb=0 THEN doA 'pDb=0 as soon as a behaviour is processed in doBeh: or on Powerup/Reset doBeh: pDa =pD 'save pointer pD =pDb 'new behavior to do pDb =0 'don't do it again stackcmnd =z 'discard stack moves and move params reducehurts: hurtsR =hurtsR MIN hurtdone -hurtdone 'don't cross 0 hurtsL =hurtsL MIN hurtdone -hurtdone 'don't cross 0 doA: IF pD<>0 THEN readpot 'carry on, pD=0 on Powerup/Reset IF pDa=0 THEN _defaultA 'nothing to finish off so do default, pDa=0 on Powerup/Reset pD =pDa 'an unfinished act after a bump pDa =0 'so nothing to finish off GOTO readpot _defaultA: DEBUG"dA " READ defaultAct,pD fB_EyesWOn =0 'don't continue with past behaviours in default Act fB_StOn =0 '------------------------------------------- readpot: HIGH potpin PAUSE 1 RCTIME potpin,1,potval '*************determine rollC*************** 'swap rollC con xxx for rollC byte ' rollC =150+(potval/40) 'DEBUG ?potval,?rollC 'potval=205 'uncomment for speed control section next '*************adjust speed****************** getspeed: potspeed =potval/30 MIN 1 MAX 15 'potvalue max=~480 =>1 - 15, keep within a nibble 'This will set speed until S used, also sets max speed in _S for auto-increment IF fs_progspeed=1 THEN gotspeed '1=>pgm sets speed and adjusts it towards potspeed pacespeed = potspeed rollspeed =pacespeed/pacerollspeedratio MIN 1 gotspeed: 'DEBUG CR,?potval,?pacespeed,?rollspeed DEBUG "s",DEC pacespeed '******************************************* 'docmnd: ' f_Sbdo =1 'enable rentrant behaviours cmnd =stackcmnd 'stackcmnd=0 on Powerup/Reset stackcmnd =Z IF cmnd<>z THEN cmndCASE READ pD,cmnd 'DEBUG "@",DEC pD,";",DEC cmnd pD =pD +1 cmndCASE: 'DEBUG "c",DEC cmnd," " ',dec f_Sbdo," ",dec f_mode,cr 'DEBUG "case",?f_speed,?potval,?pacespeed,?rollspeed 'DEBUG "CASE ",?Pd,?cmnd 'Action cmnds BRANCH cmnd,[_Z,_F,_B,_R,_L,_H,_V,_Br,_Bl,_fT,_mT,_oT,_Vt,_Vb] 'Behavioural cmnds 'DEBUG ?cmnd cmnd =cmnd -20 BRANCH cmnd,[_Su,_Sd,_tbOn,_tbOff,_bk,_Wlight,_Wdark,_Ewoff,_ttyON,_ttyOff,_Ew,_h0,_usOn,_usOff] 'Parameter cmnds cmnd =cmnd -20 READ pD,param pD =pD +1 'point past param BRANCH cmnd,[_w,_a,_s,_m,_d,_usa,_IfusOn,_IfusL,_IfusLm,_tty,_IfHLR,_IfHR,_IfHL] 'illegal value =>fall through 'DEBUG"illegal" GOTO init _Z: 'zilch end of list pD =0 GOTO START _F: 'Forward DEBUG "F"'@",DEC paceLat fB_Fdir =1 IF paceLat>paceC THEN _Fr 'moverightfoot else moveleft 'if = then left _Fl: 'move left foot forwards ' paceLto =LfF READ LFF,paceLto GOTO R_roll _Fr: 'move right foot forwards ' paceLto =RfF READ RfF,paceLto GOTO L_roll _B: 'Back DEBUG"B" fB_Fdir =0 IF paceLatpaceC THEN _R 'so we never do StepandTurn for Tf 'else drop through to _L: _L: 'LeftTurn DEBUG "L" IF paceLat>paceC THEN StepandTurn fm_TurnedR =0 ' paceLto =LfF READ LfF,paceLto GOTO C_roll _R: 'RightTurn DEBUG "R" IF paceLat turn left 'light is +ve '|eyeL -eyeR| is max of 15, *6 =90 =max pace, ie 0-90 (what? & 245-255 ?what) 'Ltemp =eyeL *6 *eWgain 'convert to 10ms *gain 'Rtemp =eyeR *6 *eWgain 'convert to 10ms *gain paceturn =ABS(eyeL -eyeR) paceturn =paceturn *6 *eWgain MAX 50' so servomax=200 +paceturn <255 IF eyeLparam THEN WRITE defaultAct,param 'only WRITE if different GOTO START _usa: 'set USAware range(<=) to next byte 'Behaviour USAwareAt =param GOTO START _IfusOn: IF fB_usOn=0 THEN Start 'no action, just carry on pD =param 'jump to us actions GOTO Start _IfusL: 'if USRange<= jump to Act in next+1 byte 'Behaviour 'DEBUG "us" GOSUB _GetUS IF USrange>param THEN __usskipAct 'DEBUG "US-act" READ pD,pD 'do new Behavioral Act GOTO __ussave _IfusLm: 'if USRange<=USlast jump to Act in next byte 'Behaviour GOSUB _GetUS IF USrange>USmem THEN __ussave pD =param GOTO __ussave __usskipAct: pD =pD +1 'skip over Act __ussave: USmem =USrange 'save range GOTO START 'Multi parameter _tty: IF param="¬" THEN START IF fB_tty=1 THEN DEBUG param READ pD,param pD =pD +1 'point to next parameter GOTO _tty 'Tactile rules _IFhLR: IF (hurtsR+hurtsL)< param THEN ignoreBActparam GOTO BActparam _IFhR: IF hurtsR < param THEN ignoreBActparam GOTO BActparam _IFhL: IF hurtsL < param THEN ignoreBActparam BActparam: READ pD,pD 'new Behavioral Act GOTO START ignoreBActparam: pD =pD +1 GOTO START '-------------------- _GetUS: 'HIGH SRF05V 'SRF05 supply voltage Mom 'PAUSE 80 'min 73 Mom SRF05 = 0 'start with pin low PULSOUT SRF05, 5 'issue 10uS trigger pulse (5 x 2uS) PULSIN SRF05, 1,USrange 'measure echo time 'LOW SRF05V ' USrange = USrange/29 'convert to cm (divide by 74 for inches) USrange = USrange/148 MAX 15 '2" resolution (/74/2) better for a nibble DEBUG "US",DEC USrange RETURN '------- end of branch (CASE movetype) 'RollSet: '[roll servo dependent] L_roll: 'from - F, B, H READ rollL,rollRto GOTO moveservos C_roll: 'from - R, L rollRto =rollC GOTO moveservos R_roll: 'from - F, B, H READ rollR,rollRto 'GOTO moveservos moveservos: 'move servos in units of 10us FOR loops Joe PcAxe18Actor specific 'remember positions in 'atRroll' and 'atRpace' to 10us 'IF fs_servosOff THEN Qhurts turnpace: IF paceturn=0 THEN FORr stackcmnd =cmnd 'do original cmnd after paceturn rollRto =rollC 'turn a bit from where we are, paceTurn(10ms) has max of 15 *6 =90 'DEBUG ?paceturn 'DEBUG ?paceLat 'do it in two stages so -ve works - this goes really wrong for large |paceturn| READ LfF,maxval READ LfB,minval 'DEBUG ?paceturn paceLto =paceLat +paceTurn '+ve turn left 'DEBUG ?paceLto paceLto =paceLto MAX maxval MIN minval paceturn =0 'don't do it again '--------roll onto foot FORr: FOR temp=rollRat TO rollRto STEP rollspeed rollRat =temp 'note - temp gets incremented past rollRto, so can't do rollRat=rollRat PULSOUT servoroll,rollRat*5 'rollRat*5 '10uS to 2uS for BS2 PULSOUT servopace,paceLat*5 'paceLat in 10uS units 'SERIN PicAxe18Actor,N4800,[WAIT(255)] 'don't need the rest of data, and takes code space PAUSE tweenrollpulse rnxt: NEXT '--------move other foot FORp: FOR temp=paceLat TO paceLto STEP pacespeed paceLat =temp 'note - temp gets incremented past paceLto, see above note PULSOUT servoroll,rollRat*5 'rollRat in 10uS units PULSOUT servopace,paceLat*5 'paceLat*5 '10uS to 2uS for BS2 IF fB_StOn=0 THEN PAUSEpace SEROUT 0,N9600,["B"] SERIN 0,N9600,50,p_notoe,[Ttoe] LOW 1 IF Ttoe>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,[Ttoe] IF Ttoe>0 THEN S_Lhurt: 'abort move, since R Ttoe was 0 just goto S_Lhurt: GOTO PAUSESpace p_notoe: DEBUG"xpT" ' IF S_toeR_=0 THEN CalcHurts 'and abort move, toe switches active LOW ' IF fB_StOn AND NOT(S_toeL AND S_toeR) THEN CalcHurts 'and abort move, toe switches active LOW 'SERIN PicAxe18Actor,N4800,[WAIT(255)] 'don't need the rest of data, and takes code space PAUSEpace: PAUSE tweenpacepulseplus PAUSESpace: PAUSE tweenSpacepulse NEXT '--------roll upright rollRto =rollC FORu: FOR temp=rollRat TO rollRto STEP rollspeed rollRat =temp 'note - temp gets incremented past paceRto, see above note PULSOUT servoroll,rollRat*5 'rollRat in 10uS units PULSOUT servopace,paceLat*5 'paceLat in 10uS units 'SERIN PicAxe18Actor,N4800,[WAIT(255)] 'don't need the rest of data, and takes code space PAUSE tweenrollpulse NEXT Qhurts: IF fB_StOn=0 THEN START 'ignore toe sensors CalcHurts: 'S_R: SEROUT 0,N9600,["T"] SERIN 0,N9600,50,h_notoe,[Ttoe] IF Ttoe=0 THEN S_L S_Rhurt: hurtsR =hurtsR +ahurt MAX Nibmax15 S_L: ' IF S_toeL_=1 THEN Qhappy SEROUT 1,N9600,["T"] SERIN 1,N9600,50,h_notoe,[Ttoe] IF Ttoe=0 THEN Qhappy S_Lhurt: hurtsL =hurtsL +ahurt MAX Nibmax15 Qhappy: IF (hurtsL +hurtsR)=0 THEN START pDb =HurtRules 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 '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 GOTO START h_notoe: DEBUG"xhT" GOTO morehappy '------------------------ end program ------------------- ''>JoeActor-PicAxe18x.bas ' ''Memory used = 168 bytes out of 2048 ' ''18Jul11 commented out the auto iris and EyesSum ' ''----------------------------------------------------------- '' ___ ___ '' - I2.A2 |1 V 18| I1,A1 EyeL '' BS2 Sertxd,So |2 17| I0,A0,infrared EyeR '' PC Si |3 16| I7,keybdD buttonswitch '' Reset |4 15| I6,keybdCk - '' 0v |5 14| +V '' 7seg O0 |6 13| O7 7seg '' 7seg O1,IICD |7 12| O6 7seg '' 7seg O2 |8 11| O5 7seg '' 7seg O3,pwm |9____10| O4,IICK 7seg ''default resonator speed is 4MHz '' o EyeR o EyeL '' o 5v o 5v '' o 0v o 0v '' o So to BS2 o So prog '' o 5v o Si prog '' o 0v o 0v '' o 0v \ '' o 5v | Aux '' o I2 | SERIN 2,N4800_8,(wiskers) '' o I6 / 'pins =255 'all off 'symbol Act =B2 'symbol i =B3 'symbol flashed =B4 'symbol eyeR =B5 'symbol eyeL =B6 'symbol eyesLR =B7 'symbol wiskers =B8 ' ''symbol EyesSum =W5 ' ''Pins ''---- ''O0-O7 7seg ''I0 readadc 0,eyeR ''I1 readadc 1,eyeL ''I2 readadc 2 spare ''I6 spare ''I7 buttonswitch ''sertxd serout on PCpin to BS2 @4800 baud '----------------------------------------------------------- ' 'read 21,Act 'remember Act 'if pin7=0 THEN buttondown 'goto showact ' 'Start: ' flashed =0 'nextflash: ' flashed =flashed +1 ' READ Act,pins 'output 7seg code for Act ' FOR i =0 TO 50 ' IF pin7=0 THEN nextAct ' PAUSE 20 ' NEXT i ' pins =255 'all off ' PAUSE 50 ' IF flashed<5 THEN nextflash ' GOTO showAct ' 'nextAct: ' Act =Act +1 ' WRITE 21,Act 'remember Act ' IF Act<20 THEN buttondown ' Act=0 ' WRITE 21,Act 'remember Act ' 'buttondown: ' READ Act,pins ' PAUSE 50 ' IF pin7=0 THEN buttondown ' GOTO Start ' 'showAct: 'forever - tell BS2 255,Act,wiskers,eyesLR every 20ms ' readadc 0,eyeR 'Dark is high ' readadc 1,eyeL 'Dark is high ' eyeR =255 -eyeR 'invert so Bright is high ' eyeL =255 -eyeL 'invert so Bright is high ' 'Auto iris ' 'EyesSum =EyeL +eyeR ' 'reduce to Nibbles with average of 7 and max of 15 ' 'eyeL =eyeL *15 /EyesSum ' 'eyeR =eyeR *15 /EyesSum ' eyeL =eyeL /16 'scale to nibble ' eyeR =eyeR /16 'scale to nibble ' eyesLR =eyeL *16 +eyeR 'put nibbles into a byte ' READ Act,pins ' sertxd(255,Act,wiskers,eyesLR) '4*2ms=8ms ' PAUSE 10 ' sertxd(255,Act,wiskers,eyesLR) '4*2ms=8ms ' PAUSE 10 ' pins =255 'all off ' FOR i=0 TO 60 ' sertxd(255,Act,wiskers,eyesLR) '4*2ms=8ms ' PAUSE 10 ' NEXT i ' GOTO showAct ' ''(download writes to ALL locations whether declared or not) '' 76543210 'EEPROM 0, (%10100000) 'segment states, active low 'EEPROM 1, (%11111100) 'EEPROM 2, (%10000110) 'EEPROM 3, (%10000011) 'EEPROM 4, (%11010001) 'EEPROM 5, (%10001001) 'EEPROM 6, (%10001000) 'EEPROM 7, (%10110011) 'EEPROM 8, (%10000000) 'EEPROM 9, (%10010001) 'EEPROM 10,(%00100000) 'EEPROM 11,(%01111100) 'EEPROM 12,(%00000110) 'EEPROM 13,(%00000011) 'EEPROM 14,(%01010001) 'EEPROM 15,(%00001001) 'EEPROM 16,(%00001000) 'EEPROM 17,(%00110011) 'EEPROM 18,(%00000000) 'EEPROM 19,(%00010001) 'EEPROM 20,(%11111111) 'all off 'EEPROM 21,(0) 'remember Act ' 'test: 'FOR B1=0 TO 20 ' READ B1,pins ' PAUSE 200 ' NEXT B1 ' 'goto test '--------------------------------------------------------------------