'Loki_8-3.bas 'Loki_7-3.bas 'Ullr_6-3.bas 'Baldur_6-3.bas #picaxe 40x2 #slot 3 '#no_table #no_data #define keybip 'for ReadSerCmnd DISCONNECT SETFREQ m16 '#3 Memory used = 4083 bytes out of 4096 ?help, @Setup #rem '================================================== IF cmnd ="h" THEN GOSUB PrintHelp IF cmnd ="@" THEN GOSUB SETUP 'from "b@" '================================================== #endrem '================================================== Pins: '========================== SYMBOL servoLA =A.1 SYMBOL servoLH =A.0 SYMBOL servoRA =D.3 SYMBOL servoRH =D.2 SYMBOL whiskerLLED =A.6 SYMBOL whiskerRLED =A.7 SYMBOL whiskerL =A.5 'READADC needs port.pin not a symbol SYMBOL whiskerR =D.0 'READADC needs port.pin not a symbol SYMBOL EyeLLED =C.2 SYMBOL EyeRLED =C.1 SYMBOL feelerLLED =D.7 SYMBOL Lfeeler =pinD.6 'READADC needs port.pin not a symbol SYMBOL Rfeeler =pinD.5 'READADC needs port.pin not a symbol SYMBOL tipB =pinD.6 SYMBOL tipF =pinD.5 SYMBOL pin_tipB =D.6 SYMBOL pin_tipF =D.5 SYMBOL feelerRLED =D.4 SYMBOL pingpin =C.0 SYMBOL voiceLED =C.5 SYMBOL voice =C.5 SYMBOL senseLtoe =pinA.2 SYMBOL senseRtoe =pinD.1 SYMBOL inIR =A.3 SYMBOL inIRpin =pinA.3 'SYMBOL O_LCD =A.4 'bus B.7 - B.0 '================================================== VarsCons: '================================================== SYMBOL Esc =27 'Escape key SYMBOL quote2 =34 'double quote mark 'built in variables 'bptr = RAM pointer 'ptr - scratchpad pointer 'hserptr - pointer to scratchpad location of last byte received by hardware UART 'X2 parts - bit0-31 b0-55 w0-27 +56 - 255 Peek/Poke 'W0 SYMBOL sense0 =b0 SYMBOL temp0 =b0 SYMBOL f8_behaves =b1 'used in programs 0123 w=write r=read !=wr SYMBOL f_b_react =bit8 'wrr- 1=reacts on SYMBOL f_b_autosense =bit9 'w!r- 1= auto test sensors, "b$a" SYMBOL f_b_autoUp =bit10 'wrr- tested in Qfell, default=1 "bu" SYMBOL f_b_Uon =bit11 'w!R- Ultrasonic sense ON SYMBOL f_b_F =bit12 '-!-- Forward - set by Fd/Bk etc SYMBOL f_Bmodified =bit13 'w!-- behaviour settings modified 'SYMBOL f_tF =bit14 '-!-- last turn on spot 'SYMBOL f_Crawl =bit15 '!r-- 0=>be Tall, 1=>Crawl 'SYMBOL f_MoveAborted =bit15 '-!-- 'W1 SYMBOL f8_Cmnder0 =b2 SYMBOL f_warm =bit16 '!--- SYMBOL f_play =bit17 '!--- SYMBOL f_record =bit18 '!--- SYMBOL f_recordC =bit19 '!--- SYMBOL f_waitcmnd =bit20 '!--- just wait for an IRkey or Serin SYMBOL f_IRcmnd =bit21 '!r-- 1=>IRcmnd 0=>Serial/Play cmnd SYMBOL f_page0 =bit22 'w-!- 1=Acts/Slots 01-07, 0(default) =1-8 'SYMBOL f_ =bit23 ' SYMBOL f8_doServos =b3 'ww-- SYMBOL f_RAdo =bit24 '-!-! do servo SYMBOL f_RHdo =bit25 '-!-! do servo SYMBOL f_LAdo =bit26 '-!-! do servo SYMBOL f_LHdo =bit27 '-!-! do servo SYMBOL f_RApulse =bit28 'r!-r pulse servos SYMBOL f_RHpulse =bit29 'r!-r pulse servos SYMBOL f_LApulse =bit30 'r!-r pulse servos SYMBOL f_LHpulse =bit31 'r!-r pulse servos 'W2 SYMBOL W2pulse =w2 SYMBOL W2pulselo =b4 SYMBOL W2pulsehi =b5 SYMBOL W2temp =w2 'Interpage jumps for hserptr SYMBOL W2templo =b4 SYMBOL W2temphi =b5 SYMBOL W2temp0 =b4 SYMBOL W2temp1 =b5 'W3 SYMBOL cmnd =b6 SYMBOL RootCmnd =b7 SYMBOL servonum =b7 'SETUP 'W4 SYMBOL RHC =b8 'Right Hip Pace Centre for current posture SYMBOL LHC =b9 'Left Hip Pace Centre for current posture 'W5 SYMBOL RAC =b10 'Right Hip Roll Centre for current posture SYMBOL LAC =b11 'Left Hip Roll Centre for current posture 'W6 SYMBOL RHat =b12 'Servo Right Hip Pace At SYMBOL RHto =b13 'Servo Right Hip Pace To 'W7 SYMBOL LHat =b14 'Servo Left Hip Pace At SYMBOL LHto =b15 'Servo Left Hip Pace To 'W8 SYMBOL RAat =b16 'Servo Right Hip Roll At SYMBOL RAto =b17 'Servo Right Hip Roll To 'W9 SYMBOL LAat =b18 'Servo Left Hip Roll At SYMBOL LAto =b19 'Servo Left Hip Roll To 'W10 SYMBOL Sspeed =b20 '1-9 SYMBOL Sframe =b21 'Servo frame pause in ms 'W11 SYMBOL FL =b22 'Foot Lift UpDown movement SYMBOL FP =b23 'Foot Pace ForwardBack movement 'W12 SYMBOL FT =b24 'Foot Turn SYMBOL FLcmnd =b25 'Foot Lift UpDown movement 0-9 'W13 SYMBOL FPcmnd =b26 'Foot Pace ForwardBack movement 0-9 SYMBOL FTcmnd =b27 'Foot Turn movement 0-9 'W14 SYMBOL Scmnd =b28 ''Speed 1-9 SYMBOL IRcmnd =b29 'W15 SYMBOL Actptr =b30 SYMBOL ActEnd =b31 'W16 SYMBOL slot =b32 SYMBOL hframes =b33 'halt frames 'W17 SYMBOL kickcmnd =b34 SYMBOL Speedmax =b35 '1-9 'W18 SYMBOL ReactAt =b36 'React Table pointer SYMBOL TOD =b37 'Toes Offset Down 0-9 'W19 SYMBOL yawn =b38 SYMBOL standOnLR =b39 '%LR 1=down moving test toes if if <>%11 'W20 'SYMBOL topple =b40 'Thor SYMBOL usdist =b41 'prog2 'W21 SYMBOL Bo =b42 'lean back offset for backwards 'SYMBOL FLTmin =b42 'min Lift in Turns, Thor SYMBOL TOO =b43 'Toes Offset Out (Front 'Freya, Thor) 'W22 'SYMBOL W22random =w22 'b44,b45 'SYMBOL W22random0 =b44 'W23 'SYMBOL DoAt =b46 'Pointer in Do 'SYMBOL InDo =b47 'flag for In a Do 'W24 SYMBOL mindAt =b48 ' SYMBOL mindSub =b49 ' 'W25 'W26 'SYMBOL lastcmnd =b53 'W27 SYMBOL W27temp =w27 'prog1 SYMBOL W27temp0 =b54 'prog1 SYMBOL W27temp1 =b55 'prog1 SYMBOL temp54 =b54 'prog1 SYMBOL temp55 =b55 'prog1 '--------- 'Variables - Storage - peek and poke '------------------- '200 56 to 255 ($38 to $FF) SYMBOL RAM_temp0 =56 'first non variable RAM location 'SYMBOL RAM_P1_f8_flags =57 'Prog1 flags 'turned Left bit 0 '1=last turned Left, 0=Right ''SYMBOL RAM_holedetectR =58 ' ''SYMBOL RAM_holedetectL =59 ' SYMBOL RAM_obstaclesR =60 'map SYMBOL RAM_obstaclesL =61 'map SYMBOL RAM_lightR =62 'whisker ambient light level Right SYMBOL RAM_lightL =63 'whisker ambient light level Left SYMBOL RAM_UStoonear =64 SYMBOL RAM_USawareAt =65 SYMBOL RAM_USunits_cm_ =66 SYMBOL RAM_USlast =67 SYMBOL RAM_WhiskerR =68 SYMBOL RAM_WhiskerL =69 SYMBOL RAM_WhiskerRlit =70 SYMBOL RAM_WhiskerLlit =71 SYMBOL RAM_FeelerR =72 SYMBOL RAM_FeelerL =73 SYMBOL RAM_FeelerRlit =74 SYMBOL RAM_FeelerLlit =75 SYMBOL RAM_Wthresh =76 SYMBOL RAM_Fthresh =77 SYMBOL RAM_Sspeed =78 'SYMBOL RAM_randvarLo =68 'SYMBOL RAM_randvarHi =69 'SYMBOL RAM_randseedlo =70 'SYMBOL RAM_randseedhi =71 'SYMBOL RAM_LHpose =80 'SYMBOL RAM_LApose =81 'SYMBOL RAM_RHpose =82 'SYMBOL RAM_RApose =83 SYMBOL EEPROM_LHC =255 'logical value SYMBOL EEPROM_LAC =254 'logical value SYMBOL EEPROM_RHC =253 'logical value SYMBOL EEPROM_RAC =252 'logical value SYMBOL EEPROM_TOO =251 'Toes Offset Out SYMBOL EEPROM_FLcmnd =250 'Foot Lift movement 1-9 SYMBOL EEPROM_FPcmnd =249 'Pace ForwardBack movement 1-9 SYMBOL EEPROM_FTcmnd =248 'Foot Turn movement 1-9 SYMBOL EEPROM_Speeds =247 'HiNib=speedmax(9), LoNib=speed(1) SYMBOL EEPROM_TOD =246 'Toes Offset Down 'SYMBOL EEPROM_Bo =245 'Back lean back SYMBOL EEPROM_kickcmnd =244 SYMBOL EEPROM_f8_behaves =243 SYMBOL EEPROM_Wthresh =242 SYMBOL EEPROM_Fthresh =241 SYMBOL EEPROM_USawarenear =240 'start of last 16 byte slot SYMBOL EEPROM_ActsEnd =239 'end of slot 06 '================================================== 'SERVOS - ACTUAL values from .bpe - values are in 10us units 'Hips S03/2BB Ankles S06/2BB '================================================== SYMBOL SLHout = 60 ' 90 '55 servo stop SYMBOL SLHmid =150 ' 0 SYMBOL SLHin =230 ' 80 '234 pot electronics max SYMBOL SLAout =220 ' 89 '226 out hit SYMBOL SLAstand =131 ' 0 SYMBOL SLAin = 55 ' 76 '49 hit SYMBOL SRHout =245 ' 93 SYMBOL SRHmid =152 ' 0 SYMBOL SRHin = 60 ' 92 SYMBOL SRAout =65 ' 79 '55 parts hit out 50 servo stop S06 SYMBOL SRAstand =144 ' 0 SYMBOL SRAin =225 ' 81 '226 servostop S06 'Left hip out is -ve 'Left ankle out is +ve 'Right hip out is +ve 'Right ankle out is -ve '================================================== 'LOGICAL 'foot toe in is ve - if Lfd,Rbk > left foot is in front on the ground 'ankle out is +ve SYMBOL LHout =256 -SLHout '196 SYMBOL LHmid =256 -SLHmid '106 SYMBOL LHin =256 -SLHin ' 26 SYMBOL LAout =SLAout '220 SYMBOL LAstand =SLAstand '131 SYMBOL LAin =SLAin ' 55 SYMBOL RHout =SRHout '245 SYMBOL RHmid =SRHmid '152 SYMBOL RHin =SRHin ' 60 SYMBOL RAout =256 -SRAout '191 SYMBOL RAstand =256 -SRAstand '112 SYMBOL RAin =256 -SRAin ' 31 'for servo numbering in CaseAdjust: SYMBOL LH =3 SYMBOL LA =2 SYMBOL RH =1 SYMBOL RA =0 '-------------------------------------------------------------- SYMBOL DSframe =15 'default SFrame, LOOPs at ~40Hz/25ms SYMBOL Fclear =19 'minimum for toes to miss when walking SYMBOL TSO =5 'Toe Stand Offset - so toes clear each other '================================================================= ' INITIALISATION '========================== Init: hserptr =W2temp 'get back pointer IF cmnd<>"h" THEN :SERTXD("{3",RootCmnd,cmnd,",") ENDIF '================================================================= ' COMMANDER '================================================================= IF cmnd ="h" THEN GOSUB PrintHelp2 IF cmnd ="@" THEN GOSUB SETUP 'from "b@" Run0: W2pulse =hserptr 'Preserve pointer RUN 0 '================================================================= SETUP: 'Can't do this by IRremote so don't test IR '========================== 'SYMBOL RA =0 'SYMBOL RH =1 'SYMBOL LA =2 'SYMBOL LH =3 standOnLR =%00 'indeterminate - don't correct falling servonum =RA GOSUB _ShowPos DO GOSUB GetCmnd 'and update servos while waiting SELECT cmnd CASE "0" TO "3" servonum =cmnd -"0" GOSUB _ShowPos CASE "/" SELECT servonum CASE RA servonum =LA CASE La servonum =LH CASE LH servonum =RH CASE RH servonum =RA ENDSELECT GOSUB _ShowPos GOSUB Move CASE "-" f8_doServos =$F0 SELECT servonum CASE RA RAto =RAto -1 MIN RAin CASE RH RHto =RHto -1 MIN RHin CASE LA LAto =LAto -1 MIN LAin CASE LH LHto =LHto -1 MIN LHin ENDSELECT GOSUB Move CASE "+" f8_doServos =$F0 SELECT servonum CASE RA RAto =RAto +1 MAX RAout CASE RH RHto =RHto +1 MAX RHout CASE LA LAto =LAto +1 MAX LAout CASE LH LHto =LHto +1 MAX LHout ENDSELECT GOSUB Move CASE "*" GOSUB _ShowPos CASE "C" SELECT servonum CASE RA WRITE EEPROM_RAC,RAat :RAC =RAat CASE RH WRITE EEPROM_RHC,RHat :RHC =RHat CASE LA WRITE EEPROM_LAC,LAat :LAC =LAat CASE LH WRITE EEPROM_LHC,LHat :LHC =LHat ENDSELECT ENDSELECT LOOP UNTIL cmnd ="Q" OR cmnd=Esc OR cmnd="." RETURN _ShowPos: SERTXD(CR) SELECT servonum CASE LA SERTXD(" *",CR) CASE LH SERTXD(" *",CR) CASE RH SERTXD(" *",CR) CASE RA SERTXD(" *",CR) ENDSELECT' Snum SERTXD("LA",#LAat," LH",#LHat," RH",#RHat," RA",#RAat," logical",CR) SERTXD(" [",#LAat,"] [",#LHat) temp0 =256 -RHat SERTXD("] [",#temp0,"] [" ) temp0 =256 -RAat SERTXD(#temp0,"] servo",CR) RETURN '================================================================= GetCmnd: '========================== Qserial: IF hserinflag=1 THEN ReadSerCmnd 'RefreshServos: GOSUB TellServos GOTO GetCmnd ReadSerCmnd: '@@@IF f_allSerial=1 THEN cmnd =@ptrinc 'f_readAllSerial (re)set by load(0/all) "l0/a" IF ptr =hserptr THEN hserinflag =0 ENDIF '@@@ELSE '@@@ ptr =hserptr -1 'one command at a time '@@@ cmnd =@ptr '@@@ hserinflag =0 '@@@ENDIF #ifdef keybip SOUND voice,(80,7) #endif RETURN '================================================================= MOVE: '========================== 'SERTXD("move" ) f_RAdo =1 'doing all sevos f_RHdo =1 'doing all sevos f_LAdo =1 'doing all sevos f_LHdo =1 'doing all sevos LHto =LHto MIN LHin MAX LHout LAto =LAto MIN LAin MAX LAout RHto =RHto MIN RHin MAX RHout RAto =RAto MIN RAin MAX RAout 'bounce =0 'IF f_IAct=0 THEN : f_MoveAborted =0 : ENDIF '---------------------------------------------------- MoveCalcs: 'sertxd("movecalcs") 'sertxd(#f_LAdo, #f_LPdo, #f_RPdo, #f_RRdo,cr) 'sertxd("AtTo",#LPat," ",#LPto," ",#RPat," ",#RPto," ",#Sspeed) '------------- _RA: 'Right Hip Roll SELECT RAat CASE =RAto f_RAdo =0 CASE >RAto RAat =RAat MIN Sspeed -Sspeed MIN RAto CASE RHto RHat =RHat MIN Sspeed -Sspeed MIN RHto CASE LAto LAat =LAat MIN Sspeed -Sspeed MIN LAto CASE LHto LHat =LHat MIN Sspeed -Sspeed MIN LHto CASE 5 THEN AbortMove 'on faster speeds bounce never gets to 5 ' ENDIF 'f_bg_Qtoes W2pulselo = f_LAdo +f_LHdo +f_RHdo +f_RAdo 'f_XXdo =1 set by MOVE: IF W2pulselo<>0 THEN MoveCalcs ' hframes =hframes MIN 1 -1 ' IF hframes<>0 THEN MoveCalcs ' IF f_MoveAborted=1 AND f_Act=1 THEN Run0 RETURN 'AbortMove: ' toe 0=off ground -> falling? so abortMove and do SLR ' sertxd("abort") ' Sspeed =9 :f_Bmodified =1 : f_MoveAborted =1 : f_LKpulse =0: f_RKpulse =0 ' GOTO PH_SLR '================================================================= PrintHelp2: SERTXD(CR,"voice") SERTXD(CR,"T{0,1,2 talk Ali") SERTXD(CR,"v{1,b bip") SERTXD(CR," {2,e whee") SERTXD(CR," {3,o whoa") SERTXD(CR," {4,v beep100") SERTXD(CR," {5,s sshhh") SERTXD(CR," [t tsh") SERTXD(CR,"eyes - action for 0.7sec") SERTXD(CR,"eE eyes Doing") SERTXD(CR," e eyes dim") SERTXD(CR," {L,R Doing") SERTXD(CR," {l,r off") SERTXD(CR," {0-3 bit state LR bright/off") SERTXD(CR,"a,^ Actions") SERTXD(CR," {1,b bip {2,e whee {3,o whoa {4,beep") SERTXD(CR," {5,S StampRL {6,W WaveL {7,K KickOut {8,r rollroll") SERTXD(CR," {0,A be Alive {+,E experiment") SERTXD(CR," L stepL R stepR") SERTXD(CR," < kickL > kickR") SERTXD(CR,"P Position") SERTXD(CR," ") SERTXD(CR,"p Poses") SERTXD(CR," s sit no pulse") SERTXD(CR," u up lift") SERTXD(CR," f lean Forwards") SERTXD(CR," m mid") SERTXD(CR," b lean Backwards") SERTXD(CR,"t Turn") SERTXD(CR," a Away from leading foot") SERTXD(CR," t To leading foot") SERTXD(CR," m Memory, same way as last time") SERTXD(CR," o Otherway to last time") SERTXD(CR," r Right on spot") SERTXD(CR," l Left on spot") SERTXD(CR,"u{f,b Up from front,back") SERTXD(CR," {u,a auto standUp") ' SERTXD(CR,"k,\\ ") SERTXD(CR,"y{1-9 increments yawn each execution,") SERTXD(CR," if yawn=parameter then skip next cmnd") SERTXD(CR,"+ react ON, test WF sensors") SERTXD(CR,"- react ON, test G sensors") SERTXD(CR,"$ test sensors and react") SERTXD(CR," 0 reacts OFF, autosense OFF") SERTXD(CR," 1 reacts ON") SERTXD(CR," {2,a autosense ON") SERTXD(CR," {0,1 U/S react OFF,ON") SERTXD(CR," + WF") SERTXD(CR," U Ultrasonic") SERTXD(CR," W Whiskers") SERTXD(CR," F Feelers") SERTXD(CR," G Ground sensors") SERTXD(CR," T Tip sensor") SERTXD(CR,CR,"bS Save current behaves") SERTXD(CR," 0 load behaves") SERTXD(CR," I Initialise EEPROM") SERTXD(CR," L Lethargic") SERTXD(CR," N Normal") SERTXD(CR," V Vigorous") SERTXD(CR," v{l,n,h,x vitality low normal high extreme") SERTXD(CR," l{0-9 lift") SERTXD(CR," p{0-9 pace") SERTXD(CR," t{0-9 turn") SERTXD(CR," o{-,0-9 toes Offset Out") SERTXD(CR," d{0-9 toes Offset Down") 'SERTXD(CR," b,0-9 back offset") SERTXD(CR," k{0-9 kick") SERTXD(CR," u{0,1 autoUpOff,On") SERTXD(CR," $a autosense ON, [9][+]=OFF") SERTXD(CR," $1 sensors reactions ON [9][-]=OFF") SERTXD(CR," $0 sensors autosense and reactions OFF [9][+][9][-]") SERTXD(CR," U{0,1 U/S reacts OFF,ON") '## SERTXD(CR," U+{1-9 U/S AwareAt units 50mm/2in") SERTXD(CR," U-{1-9 U/S TooNear units 50mm/2in") SERTXD(CR," WS{0-9 Whisker threshold decade") SERTXD(CR," {0-9 Whisker threshold digit") SERTXD(CR," FS{0-9 Feeler threshold decade") SERTXD(CR," {0-9 Feeler threshold digit") SERTXD(CR," @ Adjust joints - / next, * print, - + adjust, C centre, Q") SERTXD(CR,CR,"?{h,?,{1-8,p,b,d,#") SERTXD(CR," print - Help,Acts,Act,Pose,BeHaves, dump- Acts,#Acts") SERTXD(CR,"?{T.U.W.F.G print- Tip,Sonar,Whiskers,Feelers,Ground") SERTXD(CR,"?{!.M print- Reacts,Mind",CR) RETURN '=================================================================