'{$STAMP BS2} '{$PBASIC 2.0} '>Loki '1E8-285 =~10*16bytes for moveservos '1E5 '#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-Dnull,Kata1-Dnull,AF3B3-Dnull,D0-Dnull =justbeep 'pins float high so default select is 3 ' 'History '------- end '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 '---- 'DoAct: ' IF cmnd>=p_null THEN NewPose ' IF cmnd>=Anull THEN NewAct '@@!! can never reach here!!! '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, ' 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 'Do '-- 'list of Acts and intrinsic Acts *, ended by 0 '*F '*B '*R '*L' '--------------------------------------------------------- ' '========================= ideas store ================================== 'Hextor codes ' b - 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 - switch to new Activity ' z - end of a Behaviour list ' s - speed, update variable Sspeed 0 - 9 ' l - leg lift update variable lift 0 - 9 ' m - move update variable body swing 0 - 9 ' h - halt for time, 1=0.1sec ' BOS deaf to HALT during this time, ' useful for direct control and voice etc ' v - voice, beep,Hz/10,ms/10 'useful???? ' r - 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, Do speed scaler '@@?? d,DoName chain 'p, pause 'h, halt 'V Voice 'v,x,x voice 'do, gosub ' ' '@@?? 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 temp VAR tempw.HIGHBYTE 'temp_DoPause VAR temp 'pausetime in 1/10 secs temp_hframes VAR temp 'halt-time in servo frames pPose VAR tempw.LOWBYTE pAct VAR Byte pDo VAR Word pDoStore VAR Word 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 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 'Other flags f_autospeed VAR Bit '1 pgm sets speed , 0 manually f_Acts2 VAR Bit 'Acts page 2, needs pAct =cmnd +Acts2 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 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, Do, - with 'x'=execute, easier to write than 'set' RAU CON 1 '-Right Ankle Up, RAD CON 2 '+Right Ankle Down, RHI CON 3 '-Right Hip In, RHO CON 4 '+Right Hip Out, LAU CON 5 '-Left Ankle Up, LAD CON 6 '+Left Ankle Down, LHI CON 7 '-Left Hip In, LHO CON 8 '+Left Hip Out, ALLp CON 9 'four servo ,,, h CON 10 'halt halt for servo frames, 0 - ~5secs 'ACTS; DO, - with 'x'=execute, easier to write than 'set' 'behaviour RAs CON 11 'Right Ankle, RHs CON 12 'Right Hip, LAs CON 13 'Left Ankle, LHs CON 14 'Left Hip, ALLs CON 15 'four servo speeds as high low nibbles in two bytes <$RH,$LA>,<$Rh,$RA> IO CON 16 'I/O control Ali CON 0 'talkAli hSon CON 17 'hole Sensors ON used before p_stand when walking, turned off by new Pose Anxt CON 18 '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 Msl CON 6 'Move_scale<$stride,lift> 'Loki $Hx,Ax 'update DATA d_JHscale,d_JAscale 1-15 10=100% 'nibble=0 =>ignore nibble s CON 7 'Speed - speed, max usable for walking is about 5 'update variable Sspeed 1 - 15, 0 auto down, 255 auto up a2 CON 8 'acts page 2, needed before every page 2 act p CON 9 'pause if 0 then random(255) 0 - 25.5secs bfs CON 10 bfsf0 CON 0 'behaviour foot sensor flag 0 bfsf1 CON 1 'behaviour foot sensor flag 1 fsQ CON 2 'foot sensor Query Dnxt CON 11 'ending at 10 means Actcmnds 11-Anxt are avilable 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 '[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 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_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" 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,Ali,z 'Rshake DATA RHO,35,RHI,35,RHO,0,z rocknroll DATA p_RL,p_RR,z Acts2 DATA @256,"Act2" 'Acts page 2 a2LFkik 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 Msl,$53,bfs,1,F,F,F,bfs,0,B,B,B,z ',FdR,BkR,z ' 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 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 a2,a2LFkik,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 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 BfsC DATA bfs,fsQ 'behaviors are referenced to Bnull to keep in a byte DATA %10000,Bup-Bnull 'all foot sensors off ground, %1xxxx so not =zero=z=end DATA %1110,BholeL-Bnull,%1101,BholeR-Bnull,%1100,BholeR-Bnull DATA %1011,BholeR-Bnull,%0111,BholeL-Bnull,%0011,BholeL-Bnull,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" 'DEBUG " D@",HEX pDo-1,"=",DEC cmnd 'DEBUG CR,"Do=",HEX cmnd,CR '#D BRANCH cmnd,[zilch,_V,_F,_B,_R,_L,_Msl,_s,_a2,_p,_bfs] 'fall through for cmnds>=Dnxt GOTO NewAct '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 '------------- _p: GOSUB read_pDoandInc_cmnd ' READ pDo,temp_DoPause ' pDo =pDo +1 pAct =Anull 'so it reads next pPose as z - it should be Anull already #### IF cmnd<>0 THEN _p1 ' IF temp_DoPause<>0 THEN _p1 READ d_randstore,cmnd RANDOM cmnd WRITE d_randstore,cmnd 'save it for next time ' READ d_randstore,temp_DoPause ' RANDOM temp_DoPause ' WRITE d_randstore,temp_DoPause 'save it for next time 'DEBUG ?temp_DoPause _p1: PAUSE cmnd *100 ' PAUSE temp_DoPause *100 GOTO Start '------------- _bfs: DEBUG"bfs" GOSUB read_pDoandInc_cmnd '6 bytes ** DEBUG DEC cmnd IF cmnd=fsQ THEN _fsQ 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 READ pDo,cmnd '6 bytes ** save 6 bytes, use gosub ' DEBUG"Q",BIN4 cmnd,"s",BIN4 INC pDo =pDo +1 '6 bytes ** IF cmnd=z THEN Start IF INC<>cmnd.LOWNIB THEN _bfsQskip 'INC are foot sensors READ pDo,cmnd pDo =Bnull +cmnd 'do a behavior Do GOTO Start _bfsQskip: pDo =pDo +1 'skip address GOTO _fsQ '------------- _Msl: GOSUB read_pDoandInc_cmnd 'parameter Stridescale,$Liftscale, ignore if a 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 READ pDo,cmnd 'parameter speed pDo =pDo +1 IF cmnd=255 THEN sinc IF cmnd=0 THEN sred GOTO _Ssdo sinc: cmnd =cmnd +2 MAX potspeed GOTO _Ssdo sred: cmnd =cmnd /2 MIN 1 _Ssdo:FOR servo=0 TO 3 Sspeed(servo) =cmnd MAX 15 NEXT GOTO ReadDo '------------- _a2: READ pDo,cmnd pDo =pDo +1 f_Acts2 =1 GOTO NewAct 'do an Act, a Bcntrl or Pose '------- subroutines ------------------- read_pDoandInc_cmnd: READ pDo,cmnd pDo =pDo +1 RETURN '---------------------------------------------------- NewAct: 'DEBUG"na" ' IF cmnd>pDo THEN START 'invalid address IF cmnd>=Anull THEN _na IF f_Acts2=1 THEN _na 'new Act in page 2 IF cmndz THEN DoAction pAct =Anull 'quit for z f_Acts2 =0 'back to Acts1, default GOTO START '------------- DoAction: IF cmnd>=Anull THEN NewAct IF cmnd>=p_null THEN NewPose _ADoCmnd: IF cmnd>8 THEN _QALLp pPose =pAct GOTO _Jpose1 _QALLp: IF cmnd>9 THEN _Qh pPose =pAct GOTO _Jpose4 _Qh: IF cmnd>10 THEN _QJs GOTO _Ah _QJs: IF cmnd>14 THEN _QALLs GOTO _bJspeed1 'do the four individual speed cmnds 11,12,13,14 _QALLs: IF cmnd=15 THEN _bJspeed4 _Qother: IF cmnd=16 THEN _IO IF cmnd=17 THEN _hSon '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 'GOTO DisplayMove '_Ah drops through to _bJspeed1: because the GOTOs were commented and weird things happened in the interpretor 'f_bfs got set to 1, even though _bfs: wasn't entered _bfs1: was, how could that happen? '------- _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 IF cmnd<>Ali THEN _eout 'only Ali as yet _talkAli: DEBUG"talk" HIGH talkpin PAUSE 5500 'cut off speech at right time '5500 INPUT talkpin _eout: GOTO ReadAct '------ _hSon: f_fs =1 '_fs =0 is done automatically after one pose GOTO ReadAct '------- subroutines ------------------- readActandInc_cmnd: IF f_Acts2=1 THEN _rai2 _rai1: READ pAct,cmnd GOTO _rai _rai2: 'DEBUG ?pAct READ pAct+256,cmnd _rai: pAct =pAct +1 RETURN '---------------------------------------------------- NewPose: pPose =cmnd 'DEBUG" P" '#D ReadPose: 'DEBUG" @",HEX pPose GOSUB readPoseandInc_cmnd 'DEBUG "=",HEX cmnd _DoPose: 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,1,2,3 ON 0,1 cmnd =cmnd *jscale /10 'MIN 1 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+6bytes, 2 =>-1 byte, 3 =>-8 bytes READ pPose,cmnd '{12 bytes pPose =pPose +1 '{ RETURN '1 byte '---------------------------------------------------- '************************* 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)=jmid THEN _LuC Sto(RA) =Sto(RA) MAX wuclear -(potspeed) ':DEBUG"ruc",CR 'potspeed for slugish servos GOTO _AdoneCu _LuC: IF Sto(LA)>=jmid THEN _AdoneCu 'quit if standing Sto(LA) =Sto(LA) MAX wuclear -(potspeed) ':DEBUG"luc",CR 'potspeed 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)=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(ra),?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_bfs=0 THEN _fsOFF 'skip if behaviors are off 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