' {$STAMP BS2pe} 'Loki-20_Cmnd.bpe '{$PBASIC 2.5} '>Loki DEBUG CR,"{1}" '1E8-285 =~10*16bytes for moveservos 'd_jointspeed DATA should be updated from Sspeed(Servo) as default for next time 'd_jointspeed DATA last used in #14 'DTi CON 1 'Default Task Index if no Tasker, Should just do what was last done? ' 'idea 'Halt - do Stand with Rfoot just ahead of Lfoot, go there on IR interupt DONE 'RM - Randomise Mindvariable byte 'DM - {DoMind} DM,, jumps forward in list of items, this doesn't need a 'z' 'if RM(mind),DM,, is default then Loki just does random forever. 'eg DM,2,T1,T2 scales Mindvariable to <0,1> and does T1 or T2 depending on value 'eg DM,4,T1,T2,T3,T4 scales Mindvariable to <0,1,2,3> and does T1 -T4 depending on value ' NewMind DATA RM,DM,T1,T2,T3,T4....,D,NewMind is better as it loops (how?) until valid Mind 'This can be used on sensor values. 'new command 'IR' from C08m2 can set wC as default then C08 can play Acts ' '------------------------------------------------------------------ 'Bugs 'left toe stays up after iStand, something to do with [Cerebellum]AdjPose: 'Msl,$1x makes the toes do funny things [Cerebellum]AdjPose: Msl,$2x is ok '------------------------------------------------------------------ 'Note 'Nose whiskers are read in Cerebellum but never acted on 'Tip sensors are read in Cerebellum but never acted on 'IRrangers don't exist '------------------------------------------------------------------ ' 'History '------- '21Apr18 Added blue eys on what were SharpIRranger 28x2pins, IRranges not now sent from Cerebellum 'Loki-20 .bpe ------------------------------------------------------ '23Mar18 rearranged for bpe and STORE 'Loki-19 .bpe ------------------------------------------------------ '20Dec17 reduced sm and sf to 2,4 '13Aug17 changed left hip servo, pot was noisy. '26Jun14 f_wtfG and f_bfs were on different bits in prog0 and prog1 - mistake '26Jan14 f_halt f_8.bit2 deleted '24Jan14 fsQ,_fsQ_fsQi,_bfsQskip,CRAM_BfsQ changed to fsA,_fsA_fsAi,_bfsAskip,CRAM_BfsA '24Jan14 BfsC changed TO BfsA (A FOR Action) '24Jan14 Action on foot sensors changed so it works, '22Jan14 added 1000uF fed through 1N4001 diode to power electronics, servos now do not brownout the electronics 'DEBUG 0,"L18",CR 'see cold: 'Loki-18 ------------------------------------------------------ '25Oct13 1CF added ss sm sf vl vn vh '5 bytes spare!!!! 'loaded to here '25Oct13 ??? added f_B and turns going backwards '18Oct13 200 Zilch and QC sections moved round, _IO rewritten 'Loki-15f ------------------------------------------------------ '19Aug13 21B Zilch and IR entries rewritten. added Si StandInitialise. '19Aug13 Reset toggle removed, not needed with IRcmnds 'Loki-15e ------------------------------------------------------ '19Aug13 1EB QDo changed to QTasker '18Aug13 1EB added IR cmnds for TVR010_PicAxe_IR_controller '18Aug13 211 added Fw,Bw,Rw,Lw,Hw,Sw because they run once and wait for a new cmnd 'Loki-15d ------------------------------------------------------ '28Jun13 219 changed Actor to 08M2, Actor no longer controlled by IREar/Commander ' now responds to changes in Do from Actor ' now responds to IR cmnds 'Loki-15c ------------------------------------------------------ '15Dec12 21A changed Task back to Do 'Loki-15b ------------------------------------------------------ '01Dec12 21B _bJspeed4: rewritten without temp '24Nov12 214 Jscale now a byte instead of DATA 'Loki-15a ------------------------------------------------------ '06Nov12 lots of changes, toe sensors LED now OFF for hole '29Oct12 xxx haltframes now a Nibble '29Oct12 xxx added 08m2 which controls Actor 5v '27Oct12 xxx changed Act5v and ActSer from 0 1 '24Jul11 1BC corrected offset in jspeed1: and in all back To Loki10 'ServoCode now in P28x 'Loki-15 ------------------------------------------------------- '04Mar11 1CD got rid of f4_Sinc AND MoveCalcs AND put INC/DEC test in MOVE '11Feb11 1A6 changed to LokiActor -change Act5v and ActSer from 0 1 after PicAxe28 does feet 'Loki-14 ------------------------------------------------------- '11Feb11 1B9 change Do to Task and pDo to pT '01Feb11 1B3 removed F,B,R,L, from Do to MetaAct, hole test only done for forward foot. 'Loki-13 ------------------------------------------------------- '27Jan11 1C3 reorganised _Qhdo:, removed f_hdo from _Ah:, AdjPose: put back slugish servo adj '(Loki-11 27Jan11 BS2sx)----------------------------------------- '(Loki-10 26Jan11 moved to BS2sx) ------------------------------- '26Jan11 1BB created CON potscale =43 for in Calcspeed: '24Jan11 1BB AdjPose: commented out -Sspeed(RA) ':DEBUG"ruc",CR 'Sspeed(RA) for slugish servos ' added debug LEDs to toe sensors, LED on = foot up, 5v -> 200R+yellowLED '24Jan11 %XXXX,BholeX corrected, _ACs: corrected to use HipPos instead of AnklePos '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 '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. '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 'Loki9 '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 '---- 'PotSetRanges won't work now 'Bup doesn't get done - set back foot to 00 not 11, then loops until front foot down - forever??!! 'nothing limits servo values other than 0-255 'needs _AclearT for turns 'at end of _JPose4 what should happen here? ' IF pPose ' h next byte is halt time in 1/10sec - 2 bytes total ' [V,....] '*F '*B '*R '*L' '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, intrinsic Acts*, and Behaviour-modifiers; ended by 0 or > '--------------------------------------------------------- ' '========================= 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 'Task 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 'D, 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 D0 D1 'IIC rear 0v 5v D0 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] EEPROMcs_ PIN 2 '3pin servo Hdr Schools-Stamp header to EEPROM x25128 16kx8 fitted EEPROMsio PIN 3 '3pin servo Hdr Schools-Stamp header to EEPROM EEPROMsck PIN 4 '3pin servo Hdr Schools-Stamp header to EEPROM SerToP28 PIN 5 '3pin servo Hdr, servoV, supplies servo power to 28x BS2ToP28rdy PIN 5 voicepin PIN 6 'Schools-Stamp header to 08cmnd.5 gotIR ready out so can tell when IR got ' means 08cmnd can play tunes on spkr 'voiceLED PIN 6 'through 1N4148 so IREar can turn on LED when IR received potpin PIN 7 SerFromP28 PIN 8 'on 3pin Hdr, [MS2] supplies 28x 5v logic P28ready PIN 8 SeroEarTDo PIN 9 'Schools-Stamp header - Sero to Ear / Sero to Tasker RdyToEarTDo PIN 9 'Schools-Stamp header - EarListen or Listening SeriEar PIN 10 EarRdy PIN 10 IRrdy PIN 10 SeriTDo PIN 11 'Schools-Stamp header <- Seri from Tasker righteye PIN 12 'Schools-Stamp header <- 3pin Hdr - Roger style eyes lefteye PIN 13 'Schools-Stamp header <- 3pin Hdr - Roger style eyes eyeleds PIN 14 'Schools-Stamp header -> 3pin Hdr - Roger style eyes talkpin PIN 15 'on 3pin Hdr, [MS2] - power for TalkAli '************************* END Machine Dependent ************************* '----- Vars/Cons -------- tempw VAR Word 'used for Pot but only between Tasks, Pause time for Ali talk temp1 VAR tempw.HIGHBYTE 'mask in _bfs:_fsQ: selection in QC: temp0 VAR tempw.LOWBYTE pPose VAR tempw.LOWBYTE 'Pose pointer pAct VAR Byte 'Act pointer pDo VAR Byte 'Do pointer cmnd VAR Byte progcmnd VAR Byte Apage VAR Nib 'Act page address 0,1 at present Dpage VAR Nib 'Do page address 0,1 at present Bpage CON 5 'Sat VAR Byte(4) 'Servo At LOGICAL - Cerebellum Sto VAR Byte(4) 'Servo To LOGICAL hframes VAR Nib 'halt-time in servo frames Jscale VAR Byte 'hip step 10 is 100%, 15 is 150% of movement JHscale VAR Jscale.HIGHNIB '3 5 ok '8 max otherwise falls over when walking JAscale VAR Jscale.LOWNIB '5 '7 '4 ok '15 max 'ankle lift '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 N9600 CON 16468 'idle low, break high Pic28xBaud CON N9600 'idle low, break high 'Cerebellum flags CerebDoC VAR Byte 'flags+Ccmnd f_C1Pose VAR CerebDoC.BIT7 '1=> BS2 singlePose, wait for byte from DEBUG & 'P28 display servovalues on LCD f_CPAdj VAR CerebDoC.BIT6 'Adjust of Servo values [Cerebellum]AdjPose: f_CtfG VAR CerebDoC.BIT5 'Foot Gsensrs 1=>ON, 0=>OFF, enables test during p_Stand, set by hSon, turned off after one pose f_CSpulse VAR CerebDoC.BIT4 '1=> pulse servos, 0=> no pulses only set in init: Ccmnd VAR CerebDoC.LOWNIB 'cmnd to CerebellumP28 f8_Sensrs VAR Byte f_whiskerL VAR f8_Sensrs.BIT7 '1=sensed f_whiskerR VAR f8_Sensrs.BIT6 '1=sensed f_tipFd VAR f8_Sensrs.BIT5 '1=tipped f_tipBk VAR f8_Sensrs.BIT4 '1=tipped f4_Gnd VAR f8_Sensrs.LOWNIB ' f_GLHeel VAR f8_Sensrs.BIT3 '0=hole, 1=on ground LED on =ground sensed ' f_GLtoe VAR f8_Sensrs.BIT2 '0=hole, 1=on ground LED on =ground sensed ' f_GRheel VAR f8_Sensrs.BIT1 '0=hole, 1=on ground LED on =ground sensed ' f_GRtoe VAR f8_Sensrs.BIT0 '0=hole, 1=on ground LED on =ground sensed f8_status VAR Byte ' f4_status VAR f8_status.HIGHNIB f_B VAR f8_status.BIT4 'direction 0=Forwards, 1=backwards, for turn direction F/B f4_statusC VAR f8_status.LOWNIB 'f_hole VAR f8_status.BIT3 'hole detected, could be done by Cerebellum f_LO VAR f8_status.BIT2 'L foot out f_RO VAR f8_status.BIT1 'R foot out f_RF VAR f8_status.BIT0 'R foot forward 'Other flags f8_flags VAR Byte ' f_ VAR f8_flags.BIT7 f_bts VAR f8_flags.BIT7 'behaviour Tip sensor, 1=act on sensor f_bws VAR f8_flags.BIT6 'behaviour Whisker sensor, 1=act on sensor f_temp VAR f8_flags.BIT5 '_Jpose1: f_inlinePose VAR f8_flags.BIT4 '1=> inline Pose f_autospeed VAR f8_flags.BIT3 '1 pgm sets speed , 0 manually f_bfs VAR f8_flags.BIT2 'foot sensor behaviour 1=>ON, 0=>OFF f_wtfG VAR f8_flags.BIT1 'waiting for fG test f_playAz VAR f8_flags.BIT0 'Play to Act end, don't interupt for new IRCommand Sspeed VAR Nib(4) 'Servo Speeds MIN 1 otherwise hangs potspeed VAR Nib pscale CON 5 'BS2 10us to 2us potscale CON 43 'BS2 'IRrangeL VAR Byte 'from Cerebellum but Sharp IR rangers not yet fitted 'IRrangeR VAR Byte 'from Cerebellum but Sharp IR rangers not yet fitted '-------- Common RAM ------------------------------------- CRAM_avoidB CON 111 CRAM_avoidLF CON 112 CRAM_avoidRF CON 113 CRAM_justbeep CON 114 CRAM_Apage CON 115 CRAM_pDoNxt_lo CON 116 CRAM_pDoNxt_hi CON 117 'interupted Do pointer CRAM_Hw CON 118 CRAM_randstore CON 119 CRAM_BfsA CON 120 CRAM_zD CON 121 'current Do, will be overwritten from Tasker and various IRcmnds such as [POWER] CRAM_D_null CON 122 CRAM_A_null CON 123 CRAM_P_null CON 124 CRAM_Tasks CON 125 CRAM_iDo CON 126 thisprog CON 127 'only works for progs 0 and 1 - why? '--------------------------------------------------------- 'cmnds to prog1 '-------------- '1 return from Cerebellum 'Ccmnds - cmnds to Cerebellum '---------------------------- Cposition CON 0 Csetspeed CON 1 Cgetsensrs CON 2 CGetIfrain CON 4 'depreciated 'error codes to Cerebellum Noreason CON 8 'sertxd("error",#Command) LCD "Error",CR,"No reason" 'not used by BS2 NoTasker CON 9 'sertxd("error",#Command) LCD "Error",CR,"No Tasker" NoIREar CON 10 'sertxd("error",#Command) LCD "Error",CR,"No IREar" '************************************************************************* '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, 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 ,,, 'ACTS Acmnd1 CON 10 'marker for first Act cmnd h CON 10 'halt halt for servo frames, 0 - 300ms, drive servos a1 CON 11 'set page 1 page reset automatically to 0 on act z ifRf CON 12 'if Right Forward used in R,B,R,L meta Acts ifRo CON 13 'if Right Out used in R,B,R,L meta Acts ifLo CON 14 'if Left Out used in R,B,R,L meta Acts ifB CON 15 'if going backwards used in R,B,R,L meta Acts 'ACTS; TASK 'behaviour hSon CON 16 'hole Sensors ON used before p_stand when walking, turned off by new Pose jspeedbase CON RAs 'for offset in jspeed1: RAs CON 17 'Right Ankle, RHs CON 18 'Right Hip, LAs CON 19 'Left Ankle, LHs CON 20 'Left Hip, ALLs CON 21 'four servo speeds as high low nibbles in two bytes <$RH,$LA>,<$Rh,$RA> Ali CON 22 'Ali talk control Pretty CON 0 'talkAli - "You think you are pretty" Prettiest CON 1 'talkAli - "You think you are pretty, I am the prettiest" Greatest CON 2 'talkAli - "...greatest" bF CON 23 'Behaviour Forward direction for turns bB CON 24 'Behaviour Backward direction for turns 'IOL 'IO pin control LOW 'IOH 'IO pin control HIGH 'IOI 'IO pin control INPUT 'Do cmnds 'z CON 0 'end of list defined above 'a1 CON 11 'Act in page 1, needed before every page 1 act Defined above in Acts V CON 25 ' Voice ( beep 1KHz for 0.1sec) D CON 26 ' goto Do in next byte Msl CON 27 ' Move_scale<$stride,lift> ' Loki $Hx,Ax ' update DATA d_JHscale,d_JAscale 1-15 10=100% ' nibble=0 =>ignore nibble s CON 28 ' Speed - speed, max usable for walking is about 5 ' update variable Sspeed MIN 1 (otherwise hangs) MAX 15 sd CON 29 'auto speed down su CON 30 'auto speed up p CON 31 ' pause if 0 then random(255) 0 - 25.5secs bfs CON 32 ' bfsf0 CON 0 'behaviour foot sensor flag 0, ignore foot sensors bfsf1 CON 1 'behaviour foot sensor flag 1, act on foot sensors fsA CON 2 'foot sensor Action zD CON 33 ' set Doindex in next byte as Default tty CON 34 'send bytes to debug until '¬' T CON 35 'force Tasker bws CON 36 'behaviour Whisker sensor, 1=act on sensor Pstart CON 37 '---------------------------------------------------------------- 'Machine Dependent sectins of program: 'AdjPose: '_Spulse: 'Servo pulse '************************************************************************* 'test: 'DEBUG ? feetdown,BIN4 INC 'GOTO test '================================================================= Init: STORE 0 IF progcmnd="A" THEN ActRead 'A=> RETURN from 2-tellCerebellum IF progcmnd="C" THEN START 'C=> RETURN from 0-Commands cold: DEBUG CR,"Loki 20",CR DEBUG"cold" f_playAz =1 'play Acts to end - don't allow IR interupts due to repeat keys pAct =0 Apage =0 GET CRAM_iDo,temp1 pDo =temp1 'initialisation Do Dpage =2 PUT CRAM_pDoNxt_hi,Dpage GET CRAM_Hw,temp1 PUT CRAM_zD,temp1 'default zD f_CPAdj =1 'USER, only set/cleared here, used in [Cerebellum]AdjPose: walking doesn't work if 0 !!! '_hSon: f_CtfG =f_bfs f_CSpulse =1 'tell Cerebellum to pulse servos 'f_CsPose 'never altered LOW BS2ToP28rdy '/ready 5 LOW RdyToEarTDo '/ready f_autospeed =0 f4_Gnd =%1111 'initialise to no hole until got data from Cerebellum FOR servo=0 TO 3 Sspeed(servo) =1 'if 0 Loki hangs, s cmnd is MIN 1 Sto(servo) =jmid 'servos to mid range =>1500us logical NEXT Jscale =$53 'GOTO PotSetRanges 'set up max,min PAUSE 100 'needs 50ms for Ear to make SeriEar low, otherwise QC is done and iDo skipped GOTO START 'and do iD initialisation Do '=========== IR Loop ============================================= QCir: 'here IF SeriEar=1 from START: and ActRead: SEROUT SeroEarTDo,N9600,["C"] SERIN SeriEar,N9600,10,NoEar,[cmnd] 'Timeout of 1 is ok, if so do default f_playAz =1 'play Acts to end, don't interupt for new IRCommand, so feet end up flat DEBUG " Qir=",DEC cmnd," " 'cmnd 0 - 8 IF cmnd<9 THEN GET CRAM_Tasks,temp1 READ temp1+cmnd,pDo GOTO ReadDo ENDIF 'cmnd >8 progcmnd ="C" RUN 0 'Commands NoEar: DEBUG"No Ear" GOTO DoZDo '=========== Zilch Loop ========================================== QTasker: 'from Zilch: pDo=0 DEBUG"Tasker",CR 'HIGH SeroEarTDo 'for playback, let Ear know finished, the Debug takes 5ms before START IF SeriTDo=0 THEN DoZDo 'No change in Tasker Do askTask: Ccmnd =NoTasker 'if no Tasker reply get Cerebellum to display error NoTasker SEROUT SeroEarTDo,N9600,["T"] SERIN SeriTDo,N9600,10,tellCerebellum,[cmnd] 'Timeout of 1 is ok getDo: DEBUG "T",DEC cmnd GET CRAM_Tasks,temp1 READ temp1+cmnd,pDo DEBUG HEX pDo PUT CRAM_zD,pDo 'set as Default Do DoZDo: DEBUG"DoDefault " GET CRAM_zD,pDo DEBUG ?pDo f_playAz =1 'disable interupt for new IRCommand at Actread '=========== Do Loop ============================================= START: DEBUG CR,"S" 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/potscale MIN 1 MAX 15 '=>1 - ~15, sets max speed in Ss() 'DEBUG "potspeed ",dec potspeed," ",cr DEBUG "s^",DEC potspeed," " 'GOTO start IF f_autospeed=1 OR potspeed=Sspeed(0) THEN donspeed '1=>pgm sets speed and adjusts it towards potspeed 'potspeed=Sspeed(0) => no change cmnd =potspeed GOTO UpdateCspeed donspeed: '================================================================= ReadDo: 'DEBUG " D",DEC Dpage,")",HEX pDo DEBUG"@",DEC Dpage,HEX pDo,"," GOSUB read_pDoandInc_cmnd DEBUG DEC cmnd 'DEBUG "=C$",HEX cmnd," "',"A",DEC Apage,")" IF cmnd=0 THEN zilch IF cmnd=a1 THEN :GOSUB read_pDoandInc_cmnd :Apage =1 :GOTO Act :ENDIF GET CRAM_P_null,temp1 IF cmnd>temp1 THEN Act 'do an Act, a Bcntrl or Pose IF cmnd0 THEN _pp GET CRAM_randstore,cmnd RANDOM cmnd PUT CRAM_randstore,cmnd 'save it for next time _pp: PAUSE cmnd *100 GOTO START '------------- _bfs: DEBUG"bfs" GOSUB read_pDoandInc_cmnd DEBUG DEC cmnd IF cmnd=bfsf1 THEN _bfs1 IF cmnd=fsA THEN _fsAi IF cmnd<>bfsf0 THEN START _bfs0: f_bfs =0 GOTO START _bfs1: f_bfs =1 GOTO START _fsAi: 'foot sensor Action initialisation_fsQi temp1 =%1100 'test right toes only IF f_RF THEN _fsA 'R foot forward temp1 =%0011 'test left toes only _fsA: 'foot sensor Query GOSUB read_pDoandInc_cmnd IF cmnd=z THEN START 'only front foot is tested, back foot is set to %11, 0 is hole 'DEBUG CR,"Q",BIN4 cmnd,"s",BIN4 f4_Gnd,"RF",DEC f_RF,"msk",BIN4 temp1 IF (f4_Gnd|temp1)<>cmnd.LOWNIB THEN _bfsAskip 'f4_Gnd are copy of foot sensors GOSUB read_pDoandInc_cmnd pDo =cmnd 'do a behavior Task GOTO START _bfsAskip: pDo =pDo +1 'skip address GOTO _fsA '------------- _zD: 'Default Do in next byte GOSUB read_pDoandInc_cmnd PUT CRAM_zD,cmnd 'DEBUG "Do=",DEC cmnd," ",HEX cmnd," " GOTO START '------------- _tty: GOSUB read_pDoandInc_cmnd IF cmnd="¬" THEN START DEBUG cmnd GOTO _tty '------------- _bws: DEBUG"bws" GOSUB read_pDoandInc_cmnd DEBUG DEC cmnd f_bws =cmnd GOTO START '------- subroutines ------------------- read_pDoandInc_cmnd: READ 256 *Dpage +pDo,cmnd pDo =pDo +1 RETURN '================================================================= Act: DEBUG "A" progcmnd ="A" IF Apage=1 THEN NewAct GET CRAM_p_null,temp1 IF cmnd=V AND Apage>1 THEN pDo =pAct :GOTO _doneAct IF cmnd=0 AND Apage>1 THEN pDo =pAct -1 :GOTO _doneAct IF cmnd<>z THEN _Action _doneAct: DEBUG "z" pAct =0 'quit for z or no current Act Apage =0 'back to Acts0, default progcmnd =" " GOTO START _Action: 'DEBUG DEC Apage,")",DEC cmnd,"," GET CRAM_A_null,temp1 IF cmnd>=temp1 THEN NewAct 'jump to new ACT GET CRAM_p_null,temp1 IF cmnd>=temp1 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 'h halt IF cmnd=11 THEN _a1 'a1 'set to page1 BRANCH cmnd-12,[_ifRF,_ifRO,_ifLO,_ifB] IF cmnd=16 THEN _hSon 'hSon _QJs: IF cmnd>20 THEN _QALLs GOTO _bJspeed1 'do the four individual speed cmnds 17,18,19,20 _QALLs: ' 21 22 23 24 cmnd =cmnd -21 ' 0 1 2 3 ON cmnd GOTO _bJspeed4,_talkAli,_bF,_bB 'unknown cmnd so quit this Act ' GET CRAM_A_null,temp1 pAct =0'temp1 'point to a z GOTO START '------- _Ah: 'halt IF pAct=0 THEN :pAct =pDo :pDo =pDo +1 :ENDIF GOSUB readActandInc_cmnd hframes =cmnd 'units are servo frames 'DEBUG"h" GOTO tellCerebellum '#14Move '------- _bJspeed1: servo =cmnd -jspeedbase 'cmnds 0 - 3 start at cmnd offset jspeedbase GOSUB readActandInc_cmnd Sspeed(servo) =cmnd MAX 15 'DEBUG DEC servo," ",?Sspeed(servo) Ccmnd =Csetspeed GOTO tellCerebellum '#14ActRead '------ _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 IF cmnd.HIGHNIB=0 THEN _lo4 'ignore if 0 Sspeed(servo) =cmnd.HIGHNIB _lo4: ' temp =cmnd.LOWNIB ' IF temp=0 THEN _njs4 'ignore if 0 ' Sspeed(servo-1) =temp IF cmnd.LOWNIB=0 THEN _njs4 'ignore if 0 Sspeed(servo-1) =cmnd.LOWNIB _njs4: NEXT Ccmnd =Csetspeed GOTO tellCerebellum '#14ActRead '-------------- '_IOL: 'DEBUG CR,"Io" ' GOSUB readActandInc_cmnd ' LOW cmnd ' GOTO ActRead '_IOH: 'DEBUG CR,"Io" ' GOSUB readActandInc_cmnd ' HIGH cmnd ' GOTO ActRead _talkAli: DEBUG" Ali " tempw =0 GOSUB readActandInc_cmnd 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" HIGH talkpin PAUSE tempw INPUT talkpin 'HiZ =remove talk circuit supply GOTO ActRead '------ _hSon: f_CtfG =f_bfs 'f_Ctfs =0 is done automatically after one pose, 'only gets set if f_bfs=1 GOTO ActRead '------ _a1: 'do a page1 act in next byte ' IF pAct=0 THEN :pAct =pDo :pDo =pDo +1 :ENDIF 'inline in a Do 'DEBUG"_a1@",HEX ?pAct Apage =1 GOSUB readActandInc_cmnd GOTO NewAct '------ _ifRF: IF f_RF THEN ActRead GOTO skipAct _ifRO: IF f_RO THEN ActRead GOTO skipAct _ifLO: IF f_LO THEN ActRead GOTO skipAct '_ifRO_: ' IF Sat(RH)<=jmid THEN ActRead ' GOTO skipAct '_ifLO_: ' IF Sat(LH)<=jmid THEN ActRead _ifB: IF f_B THEN ActRead ' GOTO skipAct skipAct: 'skip Act pAct =pAct +1 GOTO ActRead _bF: f_B =0 GOTO ActRead _bB: f_B =1 GOTO ActRead '------- subroutines ------------------- readActandInc_cmnd: READ 256 *Apage +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 GotPose IF cmnd=ALLp THEN _Jpose4 'otherwise _Jpose1: DEBUG"_Jpose1¬",DEC cmnd," " cmnd =cmnd -1 'get cmnds as 0 - 7 so sums work servo =cmnd /2 'cmnds 0 - 7, there are two cmnds per servo, eg RAu RAd f_temp =cmnd.LOWBIT GOSUB readPoseandInc_cmnd 'read position DEBUG DEC cmnd,"=" cmnd =cmnd *Jscale.LOWNIB(servo//2) /10 DEBUG DEC servo,"m",DEC cmnd,"," IF f_temp=0 THEN _PJdec 'either increment or decrement about mid _PJinc: Sto(servo) =jmid +cmnd MAX 255 'DEBUG "+" GOTO _PJend _PJdec: Sto(servo) =jmid MIN cmnd -cmnd 'DEBUG "-" _PJend: 'DEBUG DEC Sto(servo),"," IF f_inlinepose THEN GotPose GOTO ReadPose 'not an embeded Pose so read to z _Jpose4: 'DEBUG"_Jpose4" FOR servo=3 TO 0 GOSUB readPoseandInc_cmnd '5 bytes IF cmnd=0 THEN _npos4 'ignore if 0 cmnd =cmnd -jmid *Jscale.LOWNIB(servo//2) +(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 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*pscale PAUSE 20 GOTO PotSetRanges '=======================================================================