Appendix 4

index ]

Software listing for Loki

Loki9.bs2 - under development, latest version is on the main Loki page.

'{$STAMP BS2} '{$PBASIC 2.5} '>Loki '1E8-285 =~10*16bytes for moveservos '#D these debugs slow things down too much when testing fragments 'd_jointspeed DATA not needed as Sspeed(Servo) are all loaded with same value in Init, might as well be a CON ' 'routine selector 'd_Dos DATA AliTalk,Kata1,AF3B3,D0 =justbeep 'pins float high so default select is 3 ' 'idea 'RM - Randomise Mindvariable byte 'DM - {DoMind} DM,<range>,<range items> jumps forward in list of items, this doesn't need a 'z' 'if RM,DM,<param>,<paramlist> is default then loki just does random forever. 'eg DM,2,Do1,Do2 scales Mindvariable to <0,1> and does Do1 or Do2 depending on value 'eg DM,4,Do1,Do2,Do3,Do4 scales Mindvariable to <0,1,2,3> and does Do1 -Do4 depending on value ' NewMind DATA RM,DM,Do1,Do2,Do3,Do4....,D,NewMind is better as it loops until valid Mind 'This can be used on sensor values. ' 'History '------- end '07Jan11 1B4 renumbered Act, Do cmnds so ankle speeds are available to Do '13Dec10 1B4 added D to do a Do and Act: _ap1: and NewAct0: to do a page 1 act from an act '12Dec10 1D8 a big tidyup and rewrite (so inline Pose and Act cmnds work) ' pDo and pDostore made a byte; f_page f_inlinePose added, acts2 changed to acts1 'not tested '11Dec10 1E4 added YouPretty, I'mPretty to TalkAli_ '08Dec10 1EC removed f_bfs=0 in _QhS: so f_bfs doesn't go down to servo level '14Dec09 1E6 shortened code at _ADoCmnd: and default Do code '14Nov09 removed 9v battery and added 2xAA cells in series with servo 4xAA cells. 'CURRENT LOKI PGM is Loki9 '14May08 1D3 added reset toggle code, moved a2 back to Do cmnd '06Sep07 1F2 _cmnd added to subroutine names to indicate returned VAR '06Sep07 put hStand = hSon,p_Stand,z and chain to hStand instead. saved 12 DATA bytes '06Sep07 NewAct and NewPose in DoAct were in wrong order so couldn't chain Acts '14May07 1E0 in _p read_pDoandInc used, cmnd used instead of temp_DoPause '14May07 1DA sub read_pDoandInc added '28May07 changed Beeper from logic to ordinary piezo, (needed it for Cycler!) '31-10-06 added a switch on pin0,pin1 connector and d_Dos DATA entry and pDoInit to read selection '30-10-06 f_fsON made active for one Pose only, fs behaviors made re-entrant until feet safe. '16-10-06 Behavior processing added with foot sensor testing after each move and ' delay to make sure foot is down. 2 flags to turn off testing and turn off response ' not much room left '16-10-06 Loki9 inline servo calcs replaced by slower ForNext, needed room '15-10-06 hS0,hS1,f_hS added and test after servo pulses for f_hS '_p, rand, 'jscale min 1 '9-10-06 altered Servo arrays to be logical values about 150, loops slower now '8-10-06 _Jpose1 _Jpose4 indexing wrong, had / instead of // '8-10-06 Talkpin changed from 8 to 15 '04Oct06 Ax,Hx combined in Mx and cmnd values now nibbles, ignore 0 '19Sep06 in sync with Freya for code. '19Sep06 h in DO changed to p and p now pauses instead of driving servos, Acts page 2 done '18-09-06 changed ankle servos from ht-615 to SuperTec-S06, Loki works much better but angle/us is different '18-09-06 testing RightAnkleSero failed as right kicking, no load, got hot and takes current. 'CURRENT LOKI PGM is Loki8 '11-9-05 Seems to all work now. '30-8-05 Loki8 under new editor just in case I don't like it, changed case of cmnds! ' 256 -, changed TO 255 -, works better!!!! '6-1-05 speed put under behaviour and cmnd values altered '1-9-04 various mods - jmidSoffset removed, 150 changed to logical mid, ' logical joint positions added - eg actual jLHmid, logical LHmid ' cmnd =....max 255 added in _Jpose4 ' naming of flags changed to reflect number of bits '14-7-04 300 in servo reversal in _Spulse: changed to 256 '8-5-04 comments added for Roger '10-10-03 Loki7 - another new format for Acts and Poses ' but still requires BS2 to organise names in DATA ' only just larger than Loki5. ' not testd on Loki yet '1-10-03 Loki6 - new format for Acts and Pose 'CURRENT LOKI PGM is Loki5 '30-9-03 all servo values now logical, ' and servo reversal and mean-offset done at pulse time. ' Pot on board rewired so don't have reverse in software. 'Loki5 all value 10ms resolution, means won't move very slowly. 'early stuff '10ms values when scaled get clipped into 4ms byte! 'Sto and Sat need to be words! ' 'Bugs '---- 'nothing limits servo values other than 0-255 '_AclearW uses logical positions but is still all wrong for toes hitting '************************* Connectors ************************************ 'See below at Machine dependent Milford Schools Board '************************* Machine Dependent ***************************** 'Notes '----- 'SERVOS LEFT RIGHT 'Hips S3 [ToeLeft=- ToeRight=+] S1 [ToeLeft=- ToeRight=+] 'Ankles S2 [ToeUp=+ ToeDown=- ] S0 [ToeUp=+ ToeDown=- ] 'hips Supertec S032BB, ankles SuperTec S062BB ' 'LOGICAL 'Hips S3 [ToeLeft=+ ToeRight=-] S1 [ToeLeft=- ToeRight=+] 'Ankles S2 [ToeUp=- ToeDown=+ ] S0 [ToeUp=- ToeDown=+ ] '************************* END Machine Dependent ************************* ' 'Pot - 1 - 633 see PIN7 18Sep06, Ambler board gives 1 - 745 '--------------------------------------------------------- 'Pose '---- 'all Sto() are scaled by user-vars @d_JAscale, @d_JHscale 'NOTYET and maxed by amax,hmax 'xAs,xHs, Alls are Act or Do{inline-Act} cmnds 'RAU,RAD,RHI,RHO,LAU,LAD,LHI,LHO - next byte is offset 0 is end of list 'RAU,5 etc, scale as % so scale 80 is 80% and 110 is 110% 'ALLp next four bytes are positions absolute - 5 bytes total ' - Pose is 4 bytes - S3,S2,S1,S0 = LH,LA,RH,RA ' if value=0 then ignore it, ie don't change Sto(x) '?##am = 14 min for feet off ground, 40 ~max otherwise feet hit ankles ' values are +- about mid '?##hm max 25 otherwise toes hit ankles ' values are +- about mid 'Action - from one Pose to another '------ 'an Action is the movement to:- ' 1. a joint setting - eg RAU,5 ' 2. a named pose - eg pStand ' RAU,RAD,RHI,RHO,LAU,LAD,LHI,LHO,Allp ' 3. a control action - IO,<cmnd type> ' h next byte is halt time in 1/10sec - 2 bytes total ' [V,....] 'Behaviour-modifier '------------------ 'Bc next byte is Behavior control - 2 byte total 'RAs,RHs,LAs,LHs next byte is speed - 2 byte total 'ALLs next two bytes are speed - 3 bytes total ' if value=0 then ignore it, ie don't change Ss(x) 'Msl MoveScale<$stride,lift> Do cmnd - 2 byte total ' if value=0 then ignore it 'Act '--- 'list of Poses, and Behaviour-modifiers; ended by 0 or <Act> 'Do '-- 'list of Acts, intrinsic Acts *, and Behaviour-modifiers; ended by 0 or <D,<Do>> '*F '*B '*R '*L' '--------------------------------------------------------- ' '========================= ideas store ================================== 'Hextor codes ' b<Behave> - new Behavior ' H - Halt ' S - Stand ' A - stand to Attention ' E - stand Easy, turn off servos ' N - goto Neutral position, current values held in EEPROM ' T - Terminal mode, listen to terminal ' V - Voice, beep 1KHz for 0.1sec ' Z - Zzzz all servos to sleep, stop pulsing ' a<dec Do> - switch to new Activity ' z - end of a Behaviour list ' s<dec Servo,dec Sspeed> - speed, update variable Sspeed 0 - 9 ' l<dec lift> - leg lift update variable lift 0 - 9 ' m<dec swing> - move update variable body swing 0 - 9 ' h<dec sec*10> - halt for time, 1=0.1sec ' BOS deaf to HALT during this time, ' useful for direct control and voice etc ' v<dec Hz/10,dec ms/10> - voice, beep,Hz/10,ms/10 'useful???? ' r<dec legdown> - rideheight update variable legdown ' W - Wait ' 'ideas '===== 'the first value of a DO,ACT,POSE,CTRL,BEHAVIOUR ' is a type value "D","A","P","C","B", last value is 0 ' then @@?? below not needed. --- not quite right yet ' with prebyte don't need to avoid overlapping cmnds and 'data addresses 'only need one prebyte = execute-name. --don't like this 'POSE '---- 'sp,x,x,x,x servo positions 'sp0,x servo0 position 'sp1,x servo1 position 'sp2,x servo2 position 'sp3,x servo3 position 'ACT can be chained '--- 'sp0,x servo0 position 'sp1,x servo1 position 'sp2,x servo2 position 'sp3,x servo3 position 'ss,x servo speeds 'ss0,x servo0 speed 'ss1,x servo1 speed 'ss2,x servo2 speed 'ss3,x servo3 speed '@@?? p,PoseName include 'then don't have to separate them in EEPROM '@@?? a,ActName chain 'then don't have to separate them in EEPROM 'h,x halt 'V Voice 'v,x,x voice 'DO can be chained, can be included ' '-- *meta acts '*F '*B '*R '*L 'Msl,<$sl> Do position scaler, s=stride, l=lift 's,<speed> Do speed scaler '@@?? d,DoName chain 'p,<x> pause 'h,<x> halt 'V Voice 'v,x,x voice 'D,<do> gosub<do> '<Act> '<Pose> '@@?? b,BehaviorName ' spx,x%,x%,x%,x% servo position scaler ' ssx,x%,x%,x%,x% servo speed scaler 'BEHAVIOR '-------- 'ss,x,x,x,x servo speed 'sensors state =>behaviorX ' behaviorX => start the new Act, or for hormones gosub a Do ' ss,x,x,x,x servo speed ' a,ActName ' d,DoName '--------------------------------------------------------- 'I/O '--- 'use the address as a goto a subroutine. '===================== end ideas store ================================== ' '************************* Machine Dependent ***************************** 'Milford SchoolsBS2 board =MS2 'view from rear 'Bus 9v 5v D7 D6 D5 D4 D3 D2 D1 D0 0v /MR 'sensors 0v 5v Dx 'servos D5 5v 0v 'servos D4 5v 0v 'servos D3 5v 0v 'servos D2 5v 0v 'IIC front 0v 5v D1 D1 'IIC rear 0v 5v D1 D1 'umbilical power -- 0v -- -- -- 6v must be on front connector 'Battery & Feet front 3v 0v 5v D0 D1 6v 'Battery & Feet rear 0v 0v 5v D0 D1 3v '----- Pins -------- 'spare CON 0 'I2C,Serout - on 4pin Hdr 0v,5v,pin0,pin1, [MS2] 'spare CON 1 'I2C,Serin - on 4pin Hdr 0v,5v,pin0,pin1, [MS2] 'pin0 Do selection on 4pin Hdr 0v,5v,pin0,pin1, [MS2] 'pin1 Do selection on 4pin Hdr 0v,5v,pin0,pin1, [MS2] RAsp CON 2 'RA - Right Ankle RHsp CON 3 'RH - Right Hip LAsp CON 4 'LA - Left Ankle LHsp CON 5 'LH - Left Hip voicepin CON 6 potpin CON 7 'Loki, Milford SchoolsBS2 board [MS2], 1 - 633, 18Sep06 'INC - pins 8,9,10,11 when foot is in air switches are closed and Dx are switched to 0v, ' when foot is down switches are open and Dx pulled to 5v by 100k 'rightfronttoe pin8 'rightfrontheel pin9 'leftfronttoe pin10 'leftfrontheel pin11 'eyeleds pin12 Roger style eyes 'righteye pin13 'lefteye pin14 talkpin CON 15 'spare CON 14 'on 3pin Hdr, [MS2] 'spare CON 15 'on 3pin Hdr, [MS2] - used for TalkAli '************************* END Machine Dependent ************************* '----- Vars/Cons -------- tempw VAR Word 'used for Pot but only between DOs, PAuse time for Ali talk temp VAR tempw.HIGHBYTE 'cmnd VAR 'pausetime in 1/10 secs temp_hframes VAR temp 'halt-time in servo frames pPose VAR tempw.LOWBYTE pAct VAR Byte pDo VAR Byte pDoStore VAR Byte cmnd VAR Byte jscale VAR Byte 'jmidS VAR Byte 'actual mid range (logical) jmid CON 150 'pos4 values are about scale mid of 1500us =150 servo VAR Nib feetdown VAR Nib 'Servo vars f8_Sflags VAR Byte 'servo flags f_S0inc VAR f8_Sflags.BIT0 '1 => Sat<Sto; 0 => Sat>Sto f_S1inc VAR f8_Sflags.BIT1 f_S2inc VAR f8_Sflags.BIT2 f_S3inc VAR f8_Sflags.BIT3 f4_Sinc VAR f8_Sflags.LOWNIB f_S0do VAR f8_Sflags.BIT4 '1 =>servo not yet at Pose f_S1do VAR f8_Sflags.BIT5 f_S2do VAR f8_Sflags.BIT6 f_S3do VAR f8_Sflags.BIT7 f4_Sdo VAR f8_Sflags.HIGHNIB 'if =0 then all moved and at Pose f_Spulse VAR Bit '1 =>pulse servo, 0 =>no pulses only set in init: 'Other flags f_autospeed VAR Bit '1 pgm sets speed , 0 manually f_Page VAR Bit 'page for address 0,1 at present f_sStep CON 0 '1=> singlestep & display servovalues,wait for byte from DEBUG f_hdo VAR Bit '1=>halt for frames f_fs VAR Bit 'foot sensors 1=>ON, 0=>OFF, enables test during p_Stand, set by hSon, turned off after one pose f_bfs VAR Bit 'foot sensor behaviour 1=>ON, 0=>OFF f_DoStored VAR Bit '1=>Do pointer stored in EEPROM f_inlinePose VAR Bit '1=> inline Pose Sspeed VAR Nib(4) 'Servo Speeds Sat VAR Byte(4) 'Servo At LOGICAL Sto VAR Byte(4) 'Servo To LOGICAL pscale CON 5 'BS2 10us to 2us potspeed VAR Nib '--------------------------------------------------------- 'All pose data must follow this order Rlift CON 0 Rstride CON 1 Llift CON 2 Lstride CON 3 '************************* Machine Dependent ***************************** 'Mapping - joints to pose(0 - 3) RA CON Rlift 'Right Ankle RH CON Rstride 'Right Hip LA CON Llift 'Left Ankle LH CON Lstride 'Left Hip '************************* END Machine Dependent ************************* 'ACTIONS z CON 0 'end of list '************************* Machine Dependent ***************************** 'POSE; ACTS 'idea = Do, - with 'x'=execute, easier to write than 'set' RAU CON 1 '-Right Ankle Up, <position> RAD CON 2 '+Right Ankle Down,<position> RHI CON 3 '-Right Hip In, <position> RHO CON 4 '+Right Hip Out, <position> LAU CON 5 '-Left Ankle Up, <position> LAD CON 6 '+Left Ankle Down, <position> LHI CON 7 '-Left Hip In, <position> LHO CON 8 '+Left Hip Out, <position> ALLp CON 9 'four servo <positions>,<position>,<position>,<position> 'ACTS Acmnd1 CON 10 'marker for first Act cmnd h CON 10 'halt<frames> halt for servo frames, 0 - ~5secs p1 CON 11 'set page 1 page reset automatically to 0 on act z 'ACTS; DO hSon CON 12 'hole Sensors ON used before p_stand when walking, turned off by new Pose 'spare con 13 'behaviour RAs CON 14 'Right Ankle,<Speed> RHs CON 15 'Right Hip, <Speed> LAs CON 16 'Left Ankle, <Speed> LHs CON 17 'Left Hip, <Speed> ALLs CON 18 'four servo speeds as high low nibbles in two bytes <$RH,$LA>,<$Rh,$RA> IO CON 19 'I/O control AliYouPretty CON 0 'talkAli - "You think you are pretty" AliIPretty CON 1 'talkAli - "You think you are pretty, I am the prettiest" Ali CON 2 'talkAli - "...greatest" Anxt CON 20 'DO cmnds 'z CON 0 'end of list defined above V CON 1 'Voice ( beep 1KHz for 0.1sec) F CON 2 'Forward one step B CON 3 'Backward one step R CON 4 'Right turn one step L CON 5 'Left turn one step D CON 6 'Do DO in next byte Msl CON 7 'Move_scale<$stride,lift> 'Loki $Hx,Ax 'update DATA d_JHscale,d_JAscale 1-15 10=100% 'nibble=0 =>ignore nibble s CON 8 'Speed<Sspeed> - speed, max usable for walking is about 5 'update variable Sspeed 1 - 14 sd CON 9 'auto speed down su CON 10 'auto speed up 'p1 CON 11 'page 1, needed before every page 1 act {defined abve} p CON 12 'pause<seconds/10> if 0 then random(255) 0 - 25.5secs bfs CON 13 bfsf0 CON 0 'behaviour foot sensor flag 0 bfsf1 CON 1 'behaviour foot sensor flag 1 fsQ CON 2 'foot sensor Query Dnxt CON 14 'ending at 13 means Actcmnds 13-Anxt are available in DOs 'Dnxt - Anxt, spare 'ACTUAL values from PotSetRanges - values are in 10us units 'S refers to servo 'assemble joints to as near as 150 as possible, hips straight ahead, feet flat 'ht615 Ankle servos changed for S06 18Sep06, RA ht615 servo failed 'S06 servo rotates opposite to ht615 so RA needs reversing at pulse out time 'LH need reversing at pulse out time 'RA [55]65 79 144 76 220[226]in 'RH [55]60 92 152 93 245[245]out 'LA [49]55 76 131 89 220[226]out realign 'LH [55]60 97 157 73 230[234]in S_RAmin CON 65 '55 parts hit out 50 servo stop S06 :ht615 235 '240 out, parts hit S_RAmid CON 144 'flat 255-S_RAmid =111 S06 :ht615 156 flat S_RAmax CON 220 '226 servostop S06 :ht615 100 '95 in, parts hit S_RHmin CON 60 '55 servo stop S_RHmid CON 152 S_RHmax CON 245 '245 pot electronics max S_LAmin CON 55 '49 hit S06 :ht615 80 '85 servo stop S_LAmid CON 131 'flat S06 :ht615 148 flat S_LAmax CON 220 '226 out hit S06 :ht615 225 '230 in, parts hit S_LHmin CON 60 '55 servo stop S_LHmid CON 157 S_LHmax CON 230 '234 pot electronics max 'LOGICAL THIS SECTION IS NOT RIGHT ??!! when it is add limits to Jpose '[with S03 Hips and ht615 Ankles Left servos go contra to logical direction] 'with S03 Hips and S06 Ankles RA and LH servos go contra to logical direction 'toe down is +ve 'toe forward is +ve 'RAmin CON 255 -S_RAmax =35 'note max min reversals 'RAmid CON 255 -S_RAmid '255-S_RAmid =111 35 76 111 79 190 'RAmax CON 255 -S_RAmin =190 'RHmin CON S_RHmin 'RHmid CON S_RHmid '154 'RHmax CON S_RHmax 'LAmin CON S_LAmax 'LAmid CON S_LAmid '159 'LAmax CON S_LAmin 'LHmin CON 255 -S_LHmax 'note max min reversals 'LHmid CON 255 -S_LHmid '255-S_LHmid =102 'LHmax CON 255 -S_LHmin Fuclear CON 19 'minimum up for toes to miss when walking Fsclear CON 5 'minimum 1/2 stride for toes to miss when walking wuclear CON jmid -Fuclear 'DATA START '---------- restart DATA 1 'poweron, reload =>end; reset=>init 'd_LogicalMid DATA 'dummy, in data so they can be indexed 'd_RAmid DATA RAmid 'flat 'd_RHmid DATA RHmid 'd_LAmid DATA LAmid 'flat 'd_LHmid DATA LHmid d_jointspeed DATA 'dummy following four bytes read in Init: d_RAspeed DATA 1 d_RHspeed DATA 1 d_LAspeed DATA 1 d_LHspeed DATA 1 d_Jscale DATA 'dummy Jmove scale 1-15, 10=100% - _Jpose1,_Jpose4 d_JAscale DATA 5 '7 '4 ok '15 max 'ankle lift d_JHscale DATA 3 '5 ok 'hip step '8 max otherwise falls over when walking '6 max otherwise toes hit other foot d_randstore DATA 61 'seed 'used for pause 0 'data four addresses for Dos offset from Dnull to keep in a byte, read pins0,1 to choose 'pins float high so default is 3, could read foot sensors while pin0(say)pressed 'd_Dos DATA AliTalk-Dnull,Kata1-Dnull,AF3B3-Dnull,D0-Dnull 'd_Dos DATA YPretty,AliTalk,Kata1,AF3B3,D0 'DOs in page2 so just grab low byte of address d_Dos DATA AliTalk,Kata1,AF3B3,D0 'DOs in page2 so just grab low byte of address d_end DATA 'dummy '14 data items '------Pose 'pPose pointer 'all pose values are 10us resolution DATA @Anxt,"Pose" 'don't overlap cmnds & addresses p_null DATA z 'Posture joint-cmnd,position,....,z DATA"RFU" p_wRFU DATA LAD,25,RAU,25,z 'walking Right Foot Up p_wLFU DATA LAU,25,RAD,25,z 'walking Left Foot Up p_wRFF DATA LHI,35,RHO,35,z 'walking Right Foot Forwards p_wLFF DATA LHO,35,RHI,35,z 'walking Left Foot Forwards DATA"Stand" p_Stand DATA LAD,0,RAD,0,z 'feet flat 'p_StandT DATA bST,LAD,0,RAD,0,0 'feet flat, Toes a bit down DATA"Face" p_Face DATA LHO,0,RHO,10,z 'both feet Facing forwards, ie toes in at 90deg 'with slight offset so right is in front p_tRTI DATA RHI,35,z 'turn Right Toe In p_tLTI DATA LHI,35,z 'turn Left Toe In p_tRTO DATA RHO,35,z 'turn Right Toe Out p_tLTO DATA LHO,35,z 'turn Left Toe Out 'p_RTF data RHO,10,z 'Right Toe Face 'p_LTF data LHO,0,z 'Left Toe Face p_RHM DATA RHO,0,z 'Right Toe Mid p_LHM DATA LHO,0,z 'Left Toe Mid p_HM DATA LHO,0,RHO,0,z 'both toes Mid, only use if a foot up p_RR DATA LAD,0,RAU,25,z 'Roll Right p_RL DATA LAU,25,RAD,0,z 'Roll Left p_RAM DATA RAD,0,z p_LAM DATA LAD,0,z p_Oh DATA LAU,20,RAU,20,z ' - Pose is 4 bytes - S3,S2,S1,S0 = LH,LA,RH,RA; ' this way round it is easier to visualise positions from the back 'these values are scaled about jmid=150 by jscale and shifted to about S_XXmid at pulse time p_RLUK1 DATA ALLp, 150,205,140, 0 '130,200,140, 0'RightLegUpKick p_RLUK2 DATA ALLp, 0,180,160,150 p_RLUFk1 DATA ALLp, 160,200, 0, 0'145,180, 0, 0 'RightLegUpFrontKick p_RLUFKhit DATA ALLp, 120, 0, 80, 0 p_RLUFk2 DATA ALLp, 155, 0,150,150 p_LLUFk1 DATA ALLp, 120, 0,160,200 'LeftLegUpFrontKick p_LLUFKhit DATA ALLp, 50, 0,135, 0 p_LLUFk2 DATA ALLp, 105,105,155, 0'155,155, 0, 0 'p_RLUFk3 DATA ALLp, 100, 0,100, 0 'p_RLUSR1 data ALLp, 100, 0,120, 0 'RightLegUpSweepRight 'p_RLUSR2 data ALLp, 125, 0,200, 0 'p_LLUFk1 data ALLp, 0, 0,145,180 'LeftLegUpFrontKick 'p_LLUFk2 data ALLp, 155,155, 0, 0 'p_LLUSL1 data ALLp, 120, 0,100, 0 'LeftLegUpSweepLeft 'p_LLUSL2 data ALLp, 200, 0,145, 0 '------Act DATA "Act" 'Acts page 0 Anull DATA z 'null Act pAct pointer, 'Posture address,.....,0(=illegal act/pose address) FdR DATA p_wRFU,p_wRFF,hStand 'step Forwards with Right Foot FdL DATA p_wLFU,p_wLFF,hStand 'step Forwards with Left Foot BkR DATA p_wRFU,p_wLFF,hStand 'step Backwards with Right Foot BkL DATA p_wLFU,p_wRFF,hStand 'step Backwards with Left Foot RtL DATA p_wLFU,p_HM,p_tRTI,hStand 'Turn Right moving Left footon RtR DATA p_wRFU,p_HM,p_tRTO,hStand 'Turn Right moving Right foot LtL DATA p_wLFU,p_HM,p_tLTO,hStand 'Turn Left moving Left foot LtR DATA p_wRFU,p_HM,p_tLTI,hStand 'Turn Left moving Right foot OhOh DATA p_Oh,p_RL,p_RR,z 'max lift is ahmax(=25) or feet hit Oh DATA p_Oh,z RSkik DATA ALLs,$66,$66,p_RLUK1,RAs,15,RAD,80,h,5 DATA ALLs,$11,$04,p_RLUK2 DATA ALLs,$00,$01,z RFkik DATA ALLs,$FF,$FF,p_RLUFk1,RAD,80,p_RLUFKhit,h,1 DATA ALLs,$44,$44,p_RLUFk2,ALLs,$21,$21,z hStand DATA hSon,p_Stand,z talkAli DATA IO,AliYouPretty,z 'Rshake DATA RHO,35,RHI,35,RHO,0,z rocknroll DATA p_RL,p_RR,z Acts1 DATA @256,"Act1" 'Acts page 1 a1LFkik DATA ALLs,$66,$66,p_LLUFk1,h,5,ALLs,$FF,$FF,LAD,80,p_LLUFKhit,h,5 DATA RHs,2,p_LLUFk2,ALLs,$11,$11,z 'RFKsweep DATA bSall6,pRLUFk1,RAp,230,pRLUFk3,pRLUSR1 ' data RHs,15,LHs,2,pRLUSR2,pRLUFk2,z 'LFKsweep data bSall6,pLLUFk1,pLLUFk2,pLLUSL1,LHs,15,pLLUSL2,pLLUFk2,z '------Do 'd_Dos DATA AliTalk-Dnull,Kata1-Dnull,AF3B3-Dnull,D0-Dnull 'DATA above before POSEs DATA "Do" Dnull DATA z 'null Do pDo pointer, 'next cmnd after D0 is default start after poweron or reset or '0' D0 DATA justbeep DATA V,p,10,z AF3B3 DATA su,Msl,$53,bfs,1,F,F,F,bfs,0,B,B,B,z ',FdR,BkR,z s,255, ' DATA p_RFU,p_Face,p_Stand,z DATA p_Face,p_Stand,z ' DATA R,V,R,V,B,V,B,V,L,V,V,L,V,V,V,z ' DATA Ax,80,p_Face,rocknroll,z ' DATA s,1,OhOh ',LFKsweep ' DATA V,R,R,F,L,L,B,z ' DATA a2,a2talkAli,h,25, z ' DATA p_stand,p_RFU,RSkik,RFkik,p_RFU,p_Stand,z'LFkik,p_Stand,z ' DATA 'p_stand,z'LFkik,p_Stand,z 'pretty LA133 RA65 YPretty DATA Msl,$0A,p_wRFU,RAD,35,LAD,2,talkAli,z AliTalk DATA V,talkAli,p,10,V,V,z DATA"Kata" Kata1 DATA Msl,$55,s,5,F,F,OhOh,F,OhOh,OhOh,F,talkAli 'Ax,80,Hx,80 back to default DATA Msl,$88,p_wRFU,RSkik,RFkik,p_wRFU,p_Stand,p,1 'p,1 just stops bouncing so doesnt fall over on LFkik DATA p1,a1LFkik,V,p_Stand,s,1,Msl,$55,B,B,B,V,V,V,L,L,L,p,0,z '# DATA s,1,Ax,100,Hx,100,F,F '# DATA s,1,OhOh,RSkik,s,5 '# DATA p_Stand,s,2,FdL,RFkik,z '# DATA s,2,F,RSkik,s,2,F,F,RSkik,s,1,B,B,z '# DATA p_RFF,p_RFU,p_LFU,z '#A1 DATA F,F,R,R,R,V,L,L,L,B,B,B,B,B,B,V,z '# DATA L,L,L,V,0,R,R,L,L,R,L,R,L,B,B,B,B,V,z '# DATA p_Face,p_Stand,z 'note can't forward reference addresses in DATA items so these have to come before BfsC ' it compiles ok so what happens? Bnull DATA 'dummy Bhole DATA Msl,$06,V,B,B,z 'not used BholeR DATA Msl,$07,V,B,B,L,L,z BholeL DATA Msl,$07,V,V,B,B,R,R,z Bup DATA V,p,0 'and fall through to BfsC in a loop until a foot is down, p 0 is random pause BfsC DATA sd,bfs,fsQ DATA %10000,Bup 'all foot sensors off ground, %1xxxx so not =zero=z=end DATA %1110,BholeL,%1101,BholeR,%1100,BholeR,%1011,BholeR,%0111,BholeL,%0011,BholeL,z DL DATA z 'last Do always 0 to prevent Doing program ' data p_LFUW,p_RTM,p_LTM,z 'adjust toes in/out ' data p_RFF,p_Stand,z 'adjust feet flat ' data p_RFF,p_StandT,z 'adjust toe down bias for walking '--------------------------------------------------------- 'DEBUG HEX ? p_null,HEX ? Anull, HEX ? Dnull, HEX ? D0 'DEBUG HEX ? talkali,HEX ? ali, HEX ? pface , HEX ? pStand 'DEBUG HEX ? RAD, HEX ?RAs, HEX ? p_face , HEX ? p_Stand 'DEBUG HEX ? OhOh, HEX ? p_Oh 'DEBUG HEX ?p_RL,HEX ?p_RR 'DEBUG HEX ? RSkik,HEX ? p_RLUK1,HEX ? p_RLUK2,HEX ? ALLs,HEX ? p_RLUK3,CR 'DEBUG HEX ? RFkik 'DEBUG HEX ? p_RFU 'DEBUG HEX ? p_Stand 'DEBUG HEX ? p_Face 'DEBUG HEX ? RSkik 'DEBUG HEX ? RFkik 'DEBUG HEX ? Kata1 'DEBUG HEX ? a2LFkik '************************* END Machine Dependent ************************* 'Machine Dependent sectins of program: 'AdjPose: '_Spulse: 'Servo pulse '************************************************************************* READ restart,cmnd cmnd =cmnd ^1 WRITE restart, cmnd IF cmnd=1 THEN Init DEBUG"end" END Init: DEBUG 0',"init",CR 'DEBUG ?IN0 'GOTO init f_Spulse =1 f_autospeed =0 FOR servo=0 TO 3 READ d_jointspeed+servo,Sspeed(servo) 'initialise Sspeed() Sat(servo) =jmid 'servos to mid range =>1500us logical Sto(servo) =jmid 'servos to mid range =>1500us logical NEXT LOW RAsp LOW RHsp LOW LAsp LOW LHsp 'GOTO PotSetRanges 'set up max,min pDoInit: 'DEBUG CR,BIN2 INA temp =INA & %11 'Read switches on P0,P1, switches float high DEBUG CR,"Do",DEC temp READ d_Dos+temp,pDo ' pDo =dNull +pDo ' DEBUG ?pDo pDoStore =0 START: ReadPot: HIGH potpin PAUSE 1 RCTIME potpin,1,tempw 'debug 1,"tempw ",dec tempw," ",cr Calcspeed: 'if Speed cmnds used then this will set speed until S used potspeed =tempw/43 MIN 1 MAX 15 '=>1 - ~15, sets max speed in Ss() 'DEBUG "potspeed ",dec potspeed," ",cr DEBUG CR,"s^",DEC potspeed 'goto start IF f_autospeed=1 THEN donspeed '1=>pgm sets speed and adjusts it towards potspeed FOR servo=0 TO 3 Sspeed(servo) =potspeed NEXT donspeed: ReadDo: GOSUB read_pDoandInc_cmnd 'DEBUG?pDo,?cmnd DEBUG"D",DEC cmnd,CR 'DEBUG " D@",HEX pDo-1,"=",DEC cmnd 'DEBUG CR,"Do=",HEX cmnd,CR '#D BRANCH cmnd,[zilch,_V,_F,_B,_R,_L,_D,_Msl,_s,_sd,_su,_p1,_p,_bfs] 'fall through for cmnds>=Dnxt GOTO Act 'do an Act, a Bcntrl or Pose '------------- zilch: 'DEBUG"zilch",CR IF pDoStore=0 THEN pDoInit pDo =pDoStore pDoStore =0 GOTO START '------------- _V: FREQOUT voicepin,100,4000,3300 '2800 - 5000 'DEBUG"V" GOTO START '------------- _F: IF Sat(RH)>jmid THEN _FdL 'right foot forward so move left _FdR: 'DEBUG" FdR"'@$",HEX FdR pAct =FdR GOTO ReadAct _FdL: 'DEBUG" FdL" pAct =FdL GOTO ReadAct '------------- _B: IF Sat(RH)>jmid THEN _BkR 'right foot forward so move right _BkL: 'DEBUG" BkL" pAct =BkL GOTO ReadAct _BkR: 'DEBUG" BkR" pAct =BkR GOTO ReadAct '------------- _R: IF Sat(RH)>jmid THEN _RtL 'right foot forward so move left _RtR: pAct =RtR GOTO ReadAct _RtL: pAct =RtL GOTO ReadAct '------------- _L: IF Sat(LH)>jmid THEN _LtR 'left foot forward so move right _LtL: pAct =LtL GOTO ReadAct _LtR: pAct =LtR GOTO ReadAct '------------- _D: 'Do DO in next byte GOSUB read_pDoandInc_cmnd pDo =cmnd GOTO START '------------- _p: GOSUB read_pDoandInc_cmnd IF cmnd<>0 THEN _pp READ d_randstore,cmnd RANDOM cmnd WRITE d_randstore,cmnd 'save it for next time _pp: PAUSE cmnd *100 GOTO Start '------------- _bfs: DEBUG"bfs" GOSUB read_pDoandInc_cmnd '6 bytes ** DEBUG DEC cmnd IF cmnd=fsQ THEN _fsQ '2 IF cmnd=bfsf1 THEN _bfs1 '1 IF cmnd<>bfsf0 THEN Start '0 _bfs0: 'ignore foot sensors f_bfs =0 GOTO Start _bfs1: 'test foot sensors f_bfs =1 GOTO Start _fsQ: 'foot sensor Query GOSUB read_pDoandInc_cmnd ' DEBUG"Q",BIN4 cmnd,"s",BIN4 INC IF cmnd=z THEN Start IF INC<>cmnd.LOWNIB THEN _bfsQskip 'INC are foot sensors GOSUB read_pDoandInc_cmnd pDo =cmnd 'do a behavior Do GOTO Start _bfsQskip: pDo =pDo +1 'skip address GOTO _fsQ '------------- _Msl: GOSUB read_pDoandInc_cmnd 'parameter Stridescale,$Liftscale, ignore nibble=0 _Ms: IF cmnd.HIGHNIB=0 THEN _Ml WRITE d_JHscale,cmnd.HIGHNIB _Ml: IF cmnd.LOWNIB=0 THEN Start WRITE d_JAscale,cmnd.LOWNIB GOTO Start '------------- _s: f_autospeed =1 '0=manual, 1=program GOSUB read_pDoandInc_cmnd 'parameter speed GOTO __Ssdo _sd: 'cmnd=0 f_autospeed =1 '0=manual, 1=program cmnd =Sspeed(0) /2 MIN 1 DEBUG"s-",DEC cmnd GOTO __Ssdo _su: 'cmnd=15 f_autospeed =1 '0=manual, 1=program cmnd =Sspeed(0) +1 MAX potspeed DEBUG"s+",DEC cmnd __Ssdo: FOR servo=0 TO 3 Sspeed(servo) =cmnd MAX 15 NEXT GOTO ReadDo '------------- _p1: 'do a page1 act in next byte GOSUB read_pDoandInc_cmnd f_Page =1 GOTO NewAct '------- subroutines ------------------- read_pDoandInc_cmnd: READ pDo +256,cmnd pDo =pDo +1 RETURN '---------------------------------------------------- Act: IF cmnd<p_null THEN _DoAction 'inline ACTcmnds and POSEcmnds in DOs IF cmnd<Anull THEN NewPose NewAct0: f_page =0 NewAct: pAct =cmnd DEBUG "A" ReadAct: 'Loop re-entry point until end of Poses IF pAct=Anull THEN pDo =pPose : GOTO START 'no current Act, must have been inline in Do IF f_inlinePose=1 THEN f_inlinePose =0 : pAct =pPose 'update pAct with where Read To 'DEBUG CR," @$",HEX pAct _ARead: GOSUB readActandInc_cmnd 'DEBUG "=$",HEX cmnd IF cmnd<>z THEN _Action pAct =Anull 'quit for z f_Page =0 'back to Acts0, default GOTO START '------------- _DoAction: f_Page =1 'so Do address works pAct =pDo _Action: IF cmnd>=Anull THEN NewAct0 'jump to new ACT IF cmnd>=p_null THEN NewPose IF cmnd>=Acmnd1 THEN _QAcmnds cmnd =pAct -1 'inline POSE so step back so POSE reads cmnd f_inlinePose =1 'so we can update pAct on return GOTO NewPose 'IF cmnd>8 THEN _QALLp ' pPose =pAct ' GOTO _Jpose1 '_QALLp: ' IF cmnd>9 THEN _Qh ' pPose =pAct ' GOTO _Jpose4 _QAcmnds: IF cmnd=10 THEN _Ah IF cmnd=11 THEN _ap1 'set to page1 IF cmnd=12 THEN _hSon _QJs: IF cmnd>16 THEN _QALLs GOTO _bJspeed1 'do the four individual speed cmnds 11,12,13,14 _QALLs: IF cmnd=17 THEN _bJspeed4 IF cmnd=18 THEN _IO 'unknown cmnd so quit this Act pAct =Anull 'point to a z GOTO START '------- _Ah: GOSUB readActandInc_cmnd temp_hframes =cmnd 'units are servo frames 'DEBUG"h" f_hdo =1 GOTO Move '------- _bJspeed1: servo =cmnd -11 'cmnds 0 - 3 start at cmnd offset 11 GOSUB readActandInc_cmnd Sspeed(servo) =cmnd MAX 15 'DEBUG DEC servo," ",?Sspeed(servo) GOTO ReadAct '------ _bJspeed4: FOR servo=3 TO 0 STEP 2 GOSUB readActandInc_cmnd 'DEBUG " s4=",HEX cmnd temp =cmnd.HIGHNIB IF temp=0 THEN _lo4 'ignore if 0 Sspeed(servo) =temp _lo4: temp =cmnd.LOWNIB IF temp=0 THEN _njs4 'ignore if 0 Sspeed(servo-1) =temp _njs4: NEXT GOTO ReadAct '-------------- _IO: DEBUG CR,"Io" GOSUB readActandInc_cmnd tempw =0 LOOKUP cmnd,[1300,2295,5500],tempw '1300 "So you think you're pretty" '2295 "So you think you're pretty, well I'm the prettiest" '5500 "So you think....I am the greatest" _talkAli: DEBUG"talk" HIGH talkpin PAUSE tempw INPUT talkpin GOTO ReadAct '------ _hSon: f_fs =f_bfs '_fs =0 is done automatically after one pose, only gets set if f_bfs=1 GOTO ReadAct '------ _ap1: GOSUB readActandInc_cmnd f_page =1 GOTO NewAct '------- subroutines ------------------- readActandInc_cmnd: READ 256 *f_Page +pAct,cmnd pAct =pAct +1 RETURN '---------------------------------------------------- NewPose: pPose =cmnd 'DEBUG" P" ReadPose: 'DEBUG" @",HEX pPose GOSUB readPoseandInc_cmnd 'DEBUG "=",HEX cmnd PoseCmnd: IF cmnd=z THEN AdjPose IF cmnd=ALLp THEN _Jpose4 'otherwise _Jpose1: cmnd =cmnd -1 'get cmnds as 0 - 7 so sums work temp =cmnd servo =cmnd /2 'cmnds 0 - 7, there are two cmnds per servo GOSUB readPoseandInc_cmnd 'read position READ d_Jscale+(servo//2),jscale 'servo 0,2=>0 1,3=>1 cmnd =cmnd *jscale /10 IF temp.LOWBIT=0 THEN _PJdec 'either increment or decrement about mid _PJinc: Sto(servo) =jmid +cmnd MAX 255 GOTO _PJend _PJdec: Sto(servo) =jmid MIN cmnd -cmnd _PJend: IF pPose<Anull THEN ReadPose 'not an embeded Pose so read to z GOTO AdjPose _Jpose4: FOR servo=3 TO 0 GOSUB readPoseandInc_cmnd '5 bytes IF cmnd=0 THEN _npos4 'ignore if 0 READ d_Jscale+(servo//2),jscale cmnd =cmnd -jmid *jscale +(jmid *10) /10 MAX 255 'this works even for jscale=0 'have to do it that way because of / and negative numbers - see scaletest.bs2 'scale around mid=jmid Sto(servo) =cmnd _npos4: NEXT IF pPose<Anull AND f_Page=0 THEN AdjPose 'not an embeded Pose GOTO AdjPose '------- subroutines ------------------- readPoseandInc_cmnd: READ 256 *f_Page +pPose,cmnd pPose =pPose +1 RETURN '---------------------------------------------------- '************************* Machine Dependent ***************************** AdjPose: 'DEBUG CR," LH "," LA "," RH "," RA ",CR 'DEBUG DEC Sto(LH)," ",DEC Sto(LA)," ",DEC Sto(RH)," ",DEC Sto(RA),CR _AclearW: 'make sure feet don't hit each other _ACu: 'DEBUG ?Sat(RH),?Sat(LH),?Sto(RH),?Sto(LH),?Sto(RA),?Sto(LA),CR 'these no crossing tests only adjust the end point of a crossing, ' the start point needs lifting too, ie do all poses ' IF Sat(RH)>Sat(LH) AND Sto(RH)>=Sto(LH) THEN _AdoneCu 'no crossing +Fsclear ' IF Sat(RH)<Sat(LH) AND Sto(RH)<=Sto(LH) THEN _AdoneCu 'no crossing -Fsclear _RuC: IF Sto(RA)>=jmid THEN _LuC Sto(RA) =Sto(RA) MAX wuclear -Sspeed(RA) ':DEBUG"ruc",CR 'Sspeed(RA) for slugish servos GOTO _AdoneCu _LuC: IF Sto(LA)>=jmid THEN _AdoneCu 'quit if standing Sto(LA) =Sto(LA) MAX wuclear -Sspeed(LA) ':DEBUG"luc",CR 'Sspeed(LA) for slugish servos _AdoneCu: 'DEBUG DEC Sto(LH)," ",DEC Sto(LA)," ",DEC Sto(RH)," ",DEC Sto(RA),CR,CR 'GOTO _AdoneC _ACs: 'DEBUG ?ABS(Sto(RA)-Sto(LA)) IF ABS(Sto(RH)-Sto(LH))>=Fsclear THEN _AdoneCs IF Sat(LA)<Sat(RA) AND Sto(LA)>=Sto(RA)THEN _LsC _RsC: Sto(RH) =Sto(RH) MIN (Sto(LH) +Fsclear) ':DEBUG"rsc",CR GOTO _AdoneCs _LsC: Sto(LH) =Sto(LH) MIN (Sto(RH) +Fsclear) ':DEBUG"lsc",CR _AdoneCs: _AdoneC: 'DEBUG DEC Sto(LH)," ",DEC Sto(LA)," ",DEC Sto(RH)," ",DEC Sto(RA),CR '************************* END Machine Dependent ************************* '------------- MoveCalcs: feetdown =0 'DEBUG"moveC" f4_Sdo =%1111 'doing all sevos FOR servo=0 TO 3 IF Sat(servo)>Sto(servo) THEN _MCdec _MCinc: f4_Sinc.LOWBIT(servo) =1 GOTO _MCnext _MCdec: f4_Sinc.LOWBIT(servo) =0 _MCnext: NEXT 'debug bin8 f_Sflags,cr '------------- DisplayMove: IF f_sStep=0 THEN Move ' DEBUG CR,"A@",HEX pAct," P@",HEX pPose," " DEBUG DEC Sto(3)," ",DEC Sto(2)," ",DEC Sto(1)," ",DEC Sto(0),CR'" : " ' DEBUG DEC Sspeed(3)," ",DEC Sspeed(2)," ",DEC Sspeed(1)," ",DEC Sspeed(0),CR '---------------------------------------------------- MOVE: 'debug"move",cr 'with no Lpause loops at 9 - 19 ms see above, 'don't know now Sta,Sto are arrays ' goto _S0 '------------- 'inline code is 5ms faster than this for-next loop needed the space though FOR servo=0 TO 3 _Sf: IF Sat(servo)=Sto(servo) THEN _endSf IF f4_Sinc.LOWBIT(servo)=1 THEN _incSf _decSf: Sat(servo) =Sat(servo) MIN Sspeed(servo) -Sspeed(servo) MIN Sto(servo) GOTO _Sfe _incSf: Sat(servo) =Sat(servo) +Sspeed(servo) MAX Sto(servo) GOTO _Sfe _endSf: f4_Sdo.LOWBIT(servo) =0 _Sfe: NEXT ' GOTo _Spulse '------------- '------------- _Spulse: 'Servo pulse IF f_Spulse=0 THEN _Spause 'test takes 0.3ms '************************* Machine Dependent *** Servo direction ********* 'DEBUG ?Sat(rh),?Sat(rh),?Sat(la),?Sat(lh) cmnd =(255 -(Sat(RA) -jmid)) +S_RAmid 'shift to servo mid and reverse servo PULSOUT RAsp,cmnd *pscale cmnd =Sat(RH) -jmid +S_RHmid 'shift to servo mid PULSOUT RHsp,cmnd *pscale cmnd =Sat(LA) -jmid +S_LAmid 'shift to servo mid PULSOUT LAsp,cmnd *pscale cmnd =(255 -(Sat(LH) -jmid)) +S_LHmid 'shift to servo mid and reverse servo PULSOUT LHsp,cmnd *pscale '************************* END Machine Dependent ************************* _Spause: 'Pulse pause ' PAUSE 0 'loops at ~45Hz 'all sums in _Spulse with Sto(),Sat() logical values ' PAUSE 8 'loops at ~50Hz, 60-70Hz without any pause, with Sto(),Sat() actual values ' gosub Lpause 'Cycler pause whilst listening for a command, check if pin15 low 'check if done 'debug "f_Sdo=",bin4 f_Sdo,cr IF f4_Sdo<>0 THEN MOVE _QhS: 'Query hole Sensors IF f_fs=0 OR INC=%1111 THEN _fsOFF 'INC foot hole sensors 0=>hole skip if ignoring sensors or no holes feetdown =feetdown +1 ' DEBUG ? feetdown,BIN4 INC IF feetdown <15 THEN Move 'make sure feet have time for down, 15*20ms =300ms IF pDoStore<>0 THEN _BDo 'otherwise save pDo 'DEBUG ?pDoStore pDoStore =pDo 'save main pDo _BDo: pDo =BfsC f_fs =0 'f_fsON action complete, set it OFF, default f_fs is OFF GOTO START _fsOFF: f_fs =0 'f_fsON action complete, set it OFF, default f_fs is OFF _Qhdo: 'halt for frames IF f_hdo=0 THEN _Qss temp_hframes =temp_hframes MIN 1 -1 IF temp_hframes<>0 THEN MOVE f_hdo =0 'DEBUG"~" _Qss: 'Single Step IF f_sStep=0 THEN ReadAct 'else wait for byte from DEBUG SERIN 16, $4054,20,_Spulse,[cmnd] GOTO ReadAct 'finished current Pose '------------- PotSetRanges: 'DEBUG"psr" ' servo =RAsp 'un-comment for desired servos ' servo =RHsp 'un-comment for desired servos servo =LAsp 'un-comment for desired servos ' servo =LHsp 'un-comment for desired servos 'comment following to save space HIGH potpin PAUSE 1 RCTIME potpin,1,tempw tempw =tempw /2 '1 - 633 => 0 - 317; /3=>211 actual 18Sep06 '1 - 745 => 1 - 370; /3=>230 actual DEBUG 1,DEC servo," ",DEC tempw," " '*5 to get 10us resolution PULSOUT servo,tempw*5 PAUSE 20 GOTO PotSetRanges '======================================================================= '_Idle: ' pt =ptidle ' it =it -1 ' if it<>0 then _Spulse ' if f_be_still=1 then _Spause 'Hextor ' action =255 ' lookdown cmnd,["FBRLHSUMNKZQTWVpPdDoOcCgsrmlnvwh0123456?"],action ' branchL action,[Fd,Bk,Rt,Lt,Halt,Sit,standUp,Mid,Neutral,_ ' Kill,Zzzz,GaitQ,GaitT,Waitf,Beeper,_ ' pick16,Pick4,down16,Down4,_ ' open16,Open4,close16,Close4,grippos,_ ' speedval,rideval,moveval,liftval,neutval,_ ' voice,waitservo,waittime,_ ' rxWget,rxWget,rxWget,rxWget,rxWget,rxWget,rxWget,_ ' Query] ' cmnd =255 'readPdata0: ' read P_pose,servo ' P_ptr =P_ptr +1 'donePdata0: ' if servo=15 then qhalt 'got pose so wait till done ' if P_ptr<$F000 then readPdata1 ' gosub lookupP 'read value into tox ' goto donePdata1 'readPdata1: ' read P_ptr,tox 'nxtP: P_ptr =P_ptr +1