'Freya_8-1.bas 'Freya_7-1.bas #picaxe 40x2 #slot 1 #no_table #no_data DISCONNECT SETFREQ m16 'SERTXD 19200 @m16 'HSERSETUP B19200_16,%00101 'pulsout 4MHz 10us units 'pulsout 16MHz 2.5us units '#1 Memory used = 3302 bytes out of 4096 #rem ============================================ PROG1 ----- Init: Qstanding: [Qfell:] SELECT cmnd -------- ' BEHAVIOUR NewBehave: -------- NewMapWhiskersFeelers MapWhiskerFeelers NewMapWhiskers MapWhiskers NewMapFeelers MapFeelers -------- ' SENSES QWhiskerFeelers: ReadWhiskerFeelers: QWhiskers: ReadWhiskers: QFeelers: ReadFeelers: Qground: Qfell: GetUp: 'make sure really tipped before getting up -------- ' REACTIONS Robstacle: Rhole: -------- ' ACTS WaveR: LVerticalSplits: getUpF: 'get up from on front getUpB: 'get up from on back getUpU: 'get up from upside down -------- ' ACTIONS InitLegs StampRL: StampR: StampL: KickOut: -------- MOVEMENT: Fd: Bk: Tm Tother Tt Ta Rt: Lt: Hlt: 'stand with right foot forward StandStill: 'Stand Still 'NEEDS WORK - needs foot switches for stood on foot -------- ' POSE PH_SLR: 'Stand on Left + Right PH_SR: 'Stand on Right PH_SL: 'Stand on Left 'Toes (left)(right) 0=Mid, F=Forward, B=Backward PT_0F: PT_F0: PT_0B: PT_B0: PT_BF: PT_FB: PT_HH: 'Walk Toes Halt PT_FF: PT_MS: 'Toe Mid Stand - toes offset PT_BB: -------- ' HMI bip: sshhh: 'because that is what it sounds like beep255 tshh -------- MOVE: MoveCalcs: TellServos: '@16MHz AbortMove: ============================================ PROG2 ----- SELECT RootCmnd 'CaseVoice: CASE "v" SELECT cmnd CASE "1","b" GOSUB bip CASE "2","e" GOSUB whee CASE "3","o" GOSUB whoa CASE "4","v" GOSUB beep100 CASE "5","s" GOSUB sshhh ELSE cmnd ="t" GOSUB tsh ENDSELECT ELSE SELECT cmnd CASE "?" GOSUB PrintActs f_page0 =1 :GOSUB PrintActs :f_page0 =0 CASE "^","a" GOSUB PrintSubs CASE "1" to "8" GOSUB PrintAct :f_page0 =0 CASE "b" GOSUB PrintBeHaves CASE "p" GOSUB PrintPose CASE "d" GOSUB DumpActs CASE "#" GOSUB DumpASCIIacts CASE "T" GOSUB PrintTipSensor CASE "W" GOSUB PrintWhiskerSensors CASE "F" GOSUB PrintFeelerSensors CASE "G" GOSUB PrintGroundSensors CASE "h" GOTO PrintHelp 'then redirected to #3 - not enough room here CASE "S" GOSUB Save_Behaviour ' bS CASE "0" GOSUB LoadBehaviour '"b0" and "0" from Prog0init CASE "I" GOSUB initEEPROM ' bI CASE "E" GOTO START ELSE GOSUB sshhh ENDSELECT ENDSELECT ============================================ PROG3 ----- IF cmnd="@" THEN GOSUB SetUp IF cmnd="h" THEN GOSUB PrintHelp IF cmnd="E" THEN START - experiment '************************* Machine Dependent ***************************** 'Notes '----- 'hips Supertec S062BB, ankles Supertec S03 'SERVOS LEFT RIGHT 'Hips S3 [ToeLeft=- ToeRight=+] S1 [ToeLeft=- ToeRight=+] 'Ankles S2 [ToeUp=+ ToeDown=- ] S0 [ToeUp=- ToeDown=+ ] 'hips Supertec S062BB, ankles Supertec S03 ' 'LOGICAL 'Hips S3 [ToeLeft=+ ToeRight=-] S1 [ToeLeft=- ToeRight=+] 'Ankles S2 [ToeUp=- ToeDown=+ ] S0 [ToeUp=- ToeDown=+ ] ' 'Allp logical values ' 'max speed ~12, works at 15 but feet start to catch on each other. '************************* END Machine Dependent ************************* #endrem '================================================== Pins: '========================== SYMBOL servoLA =A.1 SYMBOL servoLH =A.0 SYMBOL servoRA =D.0 SYMBOL servoRH =D.1 SYMBOL A_tipFB =A.7 'READADC needs port.pin not a symbol or pinX.Y SYMBOL whiskerLLED =C.1 SYMBOL whiskerRLED =C.2 SYMBOL whiskerL =A.5 'READADC needs port.pin not a symbol or pinX.Y SYMBOL whiskerR =A.6 'READADC needs port.pin not a symbol or pinX.Y SYMBOL feelerLLED =D.7 SYMBOL feelerL =D.6 'READADC needs port.pin not a symbol or pinX.Y SYMBOL feelerR =D.5 'READADC needs port.pin not a symbol or pinX.Y SYMBOL feelerRLED =D.4 'SYMBOL EyesSerial =C.6 'hserout SYMBOL senseLtoe =pinA.2 SYMBOL senseRtoe =pinD.3 SYMBOL senseLheel =pinA.3 SYMBOL senseRheel =pinC.0 SYMBOL voiceLED =C.5 SYMBOL voice =C.5 SYMBOL inIR =D.2 SYMBOL inIRpin =pinD.2 'SYMBOL O_LCD =A.4 'bus B.7 - B.0 '================================================== 'Reactions Table: 'SYMBOL USaware =0 TABLE USaware, ("ta.") 'SYMBOL USnear =16 TABLE USnear, ("s-Btatm.") SYMBOL OLR =32 'TABLE OLR, ("s-BBtatm.") SYMBOL OL_ =48 'TABLE OL_, ("s-BRR.") '2023-01-03 SYMBOL O_R =64 'TABLE O_R, ("s-BLL.") '2023-01-03 SYMBOL HLR =80 'TABLE HLR, ("s-BBBBtatm.") '2023-01-03 SYMBOL HL_ =96 'TABLE HL_, ("s-BRRR.") '2023-01-03 SYMBOL H_R =112 'TABLE H_R, ("s-BLLL.") '2023-01-03 SYMBOL HBLR =128 'TABLE HBLR, ("s-FF.") '2023-01-03 SYMBOL HBL_ =144 'TABLE HBL_, ("s-FFR.") '2023-01-03 SYMBOL HB_R =160 'TABLE HB_R, ("s-FFL.") SYMBOL ReactEnd =175 'TABLE ReactEnd,(".") '================================================== VarsCons: '================================================== SYMBOL mindstart =192 'table in #0 SYMBOL Esc =27 'Escape key SYMBOL quote2 =34 'double quote mark 'built in variables '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 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_Rfwd =bit11 'w!r- Right Forward 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_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 'servo pulse 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 'W4 SYMBOL RAC =b8 'Right Ankle Centre for current posture SYMBOL LAC =b9 'Left Ankle Centre for current posture 'W5 SYMBOL RHC =b10 'Right Hip Centre for current posture SYMBOL LHC =b11 'Left Hip Centre for current posture 'W6 SYMBOL LHat =b12 'Servo Left Hip At SYMBOL LHto =b13 'Servo Left Hip To 'W7 SYMBOL LAat =b14 'Servo Left Ankle At SYMBOL LAto =b15 'Servo Left Ankle To 'W8 SYMBOL RHat =b16 'Servo Right Hip At SYMBOL RHto =b17 'Servo Right Hip To 'W9 SYMBOL RAat =b18 'Servo Right Ankle At SYMBOL RAto =b19 'Servo Right Ankle To 'W10 SYMBOL Sspeed =b20 'max to 15 then to can't go past zero SYMBOL Sframe =b21 'Servo frame pause in ms 'W11 SYMBOL FL =b22 'Foot Lift UpDown movement min 25 SYMBOL FP =b23 'Foot Pace ForwardBack movement +- 9 - 23 'W12 SYMBOL FT =b24 'Foot Turn SYMBOL FLcmnd =b25 'Foot Lift movement 1-9 'W13 SYMBOL FPcmnd =b26 'Pace ForwardBack movement 1-9 SYMBOL FTcmnd =b27 'Foot Turn movement 1-9 'W14 SYMBOL Scmnd =b28 'Speed 1-9 SYMBOL IRcmnd =b29 'W15 SYMBOL Actptr =b30 SYMBOL ActEnd =b31 'W16 SYMBOL slot =b32 'Acts 1-8 occupy Slots 0-7 SYMBOL hframes =b33 'halt frames 'W17 SYMBOL kickcmnd =b34 SYMBOL Speedmax =b35 'W18 SYMBOL TOD =b36 'Toes Offset Down SYMBOL TOF =b37 'Toes Offset Front 'W19 SYMBOL yawn =b38 SYMBOL standOnLR =b39 '%LR 1=down moving test toes if if <>%11 'W20 SYMBOL topple =b40 'SYMBOL usdist =b41 'no ultrasonics 'W21 SYMBOL EyeCode =b42 SYMBOL ReactAt =b43 'React Table pointer '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 '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 'set by calibrateholewhiskers: % light level difference for hole SYMBOL RAM_holedetectL =59 'set by calibrateholewhiskers: % light level difference for hole 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 'US not fitted '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 EEPROM_LHC =255 'logical value constant LHmid transferred to EEPROM so can be changed by @ SYMBOL EEPROM_LAC =254 'logical value constant LAmid transferred to EEPROM so can be changed by @ SYMBOL EEPROM_RHC =253 'logical value constant RHmid transferred to EEPROM so can be changed by @ SYMBOL EEPROM_RAC =252 'logical value constant RAmid transferred to EEPROM so can be changed by @ SYMBOL EEPROM_TOD =251 'Toes Offset Down 0-9 SYMBOL EEPROM_TOF =250 'Toes Offset Front 0-9 'SYMBOL EEPROM_FPcmnd =249 'not Freya SYMBOL EEPROM_FLcmnd =248 'Foot Lift movement 1-9 SYMBOL EEPROM_FPcmnd =247 'Pace ForwardBack movement 1-9 SYMBOL EEPROM_FTcmnd =246 'Foot Turn movement 1-9 SYMBOL EEPROM_Speeds =245 '(hiNib)Speedmax 1-9, (loNib)Speed 1-9 SYMBOL EEPROM_kickcmnd =244 SYMBOL EEPROM_f8_behaves =243 SYMBOL EEPROM_Wthresh =242 'WhiskerThreshold SYMBOL EEPROM_Fthresh =241 'FootfeelerThreshold 'SYMBOL EEPROM_USawarenear =240 'not Freya 'start of last 16 byte slot SYMBOL EEPROM_ActsEnd =239 'end of slot 06 '================================================== 'SERVOS - ACTUAL values from PotSetRanges - values are in 10us units '========================== SYMBOL SRHmax =230 'Right Hip Max out up leg hits body SYMBOL SRHout =149 'Right Hip horizontal SYMBOL SRHmid =66 'Right Hip Stand feet flat SYMBOL SRHWmid =69 'Right Hip Mid feet flat TDO=3 SYMBOL SRHmin =50 'Right Hip Min in leg hits body SYMBOL SRAmax =255 'Right Ankle Max out servo stop SYMBOL SRAFFWmax =182 '+36 LongStep - falls over SYMBOL SRAFFWmin =176 '+30 LongStep - falls over SYMBOL SRAFWmax =169 '+23 SYMBOL SRAFWmid =162 '+16 SYMBOL SRAFWmin =155 '+9 SYMBOL SRAmid =146 'Right Ankle Mid ahead SYMBOL SRABWmin =137 '-9 SYMBOL SRABWmid =130 '-16 SYMBOL SRABWmax =122 '-24 SYMBOL SRABFWmin =115 '-31 LongStep - falls over SYMBOL SRABFWmax =111 '-35 LongStep - falls over SYMBOL SRAmin =60 'Right Ankle Min in servo stop SYMBOL SLHmax =250 'Left Hip Max in servo stop SYMBOL SLHWmid =226'230 'Left Hip Mid feet flat LOGICAL=25 TDO=7 SYMBOL SLHmid =233 'Left Hip Stand feet flat LOGICAL=22 SYMBOL SLHout =135 'Left Hip horizontal SYMBOL SLHmin =54 'Left Hip Min out up leg hits body SYMBOL SLAmin =50 'Left Ankle Min out servo stop SYMBOL SLAFFWmax =111 'LongStep - falls over SYMBOL SLAFFWmin =115 'LongStep - falls over SYMBOL SLAFWmax =122 '-24 SYMBOL SLAFWmid =130 '-16 SYMBOL SLABWmin =137 '-9 SYMBOL SLAmid =146 'Left Ankle Mid ahead SYMBOL SLAFWmin =155 '+9 SYMBOL SLABWmid =162 '+16 SYMBOL SLABWmax =169 '+23 SYMBOL SLABFWmin =176 'LongStep - falls over SYMBOL SLABFWmax =182 'LongStep - falls over SYMBOL SLAmax =220 'Left Ankle Max in servo stop 'Left toes forward is -ve 'Left leg out is -ve 'Right toes forward is +ve 'Right leg out is +ve '================================================== 'LOGICAL 'toes forward is +ve 'leg out is +ve SYMBOL RAmax =SRAmax SYMBOL RAFWmid =SRAFWmid SYMBOL RAmid =SRAmid SYMBOL RABWmid =SRABWmid SYMBOL RAmin =SRAmin SYMBOL RHmax =SRHmax SYMBOL RHout =SRHout SYMBOL RHmid =SRHmid SYMBOL RHmin =SRHmin ' 60 SYMBOL RHinmin =RHmin +5 'don't hit stop SYMBOL RHiclear=RHmid +25 SYMBOL LAmax =255 -SLAmin SYMBOL LAFWmid =255 -SLAFWmid SYMBOL LAmid =255 -SLAmid SYMBOL LABWmid =255 -SLABWmid SYMBOL LAmin =255 -SLAmax 'note max min reversals for left SYMBOL LHmax =255 -SLHmin '201 SYMBOL LHout =255 -SLHout '120 SYMBOL LHmid =255 -SLHmid ' 22 SYMBOL LHmin =255 -SLHmax ' 5 SYMBOL LHinmin =LHmin +5 'don't hit stop 'for servo numbering in CaseAdjust: SYMBOL RA =0 SYMBOL RH =1 SYMBOL LA =2 SYMBOL LH =3 '-------------------------------------------------------------- SYMBOL DSframe =15 'default SFrame, LOOPs at ~40Hz/25ms SYMBOL Fclear =20 'minimum for toes to miss when walking SYMBOL f_RfF =1 '1=>Right Foot Forward SYMBOL TSO =3 'Toe Stand Offset - so toes clear each other, only used in GetUp:,PT_FF: '================================================== ' SENSORS '========================== 'HgTilt SYMBOL TipedB =235 'A/D reading on Back SYMBOL TipB =200 'Tip B test threshold SYMBOL TipedU =167 'A/D reading Upside down SYMBOL TipU =120 'Tip B test threshold SYMBOL TipX =85 'A/D reading on Feet SYMBOL TipF =50 'Tip F test threshold SYMBOL TipedF =19 'A/D reading on Nose #rem 'nonHgTilt SYMBOL BTip =235 'A/D reading SYMBOL TipB =150 'Tip B test threshold SYMBOL NoTip =85 'A/D reading SYMBOL TipF =50 'Tip F test threshold SYMBOL FTip =19 'A/D reading #endrem #rem 'Accelerometer-KD SYMBOL BTip =59 'A/D reading SYMBOL TipB =65 'Tip B test threshold SYMBOL NoTip =74 'A/D reading SYMBOL TipF =80 'Tip F test threshold SYMBOL FTip =85 'A/D reading #endrem '================================================================= TVR010_PicAxe_IR_controller: '--------------------------- 'Before use, the transmitter must be programmed with the ‘Sony’ transmit code. '1. Insert 2 AAA size batteries, preferably alkaline. '2. Press ‘S’ and ‘B’ at the same time. S is in the centre of the arrows. ' The top left red LED should light. '3. Press ‘0’. The LED should flash. '4. Press ‘1’. The LED should flash. '5. Press ‘3’. The LED should go out. '6. Press the red power button (top right). '------------------------------------------- 'IRin cmnds PicAxe controller 'DO NOT PRESS OTHER KEYS 'ie [A] [B] [C] [D] [E] [F] [G] 'They change the Mode and [B] has to be pressed to change back. '[square] [triangle] [()] [L] [X] [backwards F] have no effect SYMBOL KEY_POWER = 21 'Sleep SYMBOL KEY_UP = 16 '[Fd] Step Forward [9][Fd]=set vitality Behaviour to faster SYMBOL KEY_DOWN = 17 '[Bk] Step Backward [9][Bk]=set vitality Behaviour to slower SYMBOL KEY_RIGHT = 18 '[Rt] Step Right SYMBOL KEY_LEFT = 19 '[Lt] Step Left SYMBOL KEY_BAR = 96 '[f|] learn an Act, next [key] selects an Act SYMBOL KEY_TENT = 54 '[f^] next [key] selects a Subroutine-Act SYMBOL KEY_VERT_CROSS = 37 'use synonym for easier coding SYMBOL KEY_VCROSS = 37 '[f+] Modify Mode {when routine included} ' [+] set USAwareAt in DUSawareAt ' [-] set UStoonear in DUStoonear ' {[Fd] alter PaceCentre in DpaceC} ' {[Bk] alter PaceCentre in DpaceC} ' {[Rt] alter RollCentre in DrollC} ' {[Lt] alter RollCentre in DrollC} ' { [-][+] to adjust } ' [Key_POWER] to quit SYMBOL KEY_DIAG_CROSS = 20 'use synonym for easier coding SYMBOL KEY_XCROSS = 20 '[fX] transfer EEPROM to PC for including in programs ' -- Subroutine-Act and Commands -- SYMBOL KEY_1 = 0 '[1] [f^][1]=>RockADo [9][1]=set speed slow SYMBOL KEY_2 = 1 '[2] [9][2]=set speed medium SYMBOL KEY_3 = 2 '[3] [9][3]=set speed fast SYMBOL KEY_4 = 3 '[4] [9][4]=set US behaviour to timid SYMBOL KEY_5 = 4 '[5] [9][5]=set US behaviour to normal SYMBOL KEY_6 = 5 '[6] [9][6]=set US behaviour to bold SYMBOL KEY_7 = 6 '[7] [9][7]=disable Whiskers test SYMBOL KEY_8 = 7 '[8] [9][8]=disable Ultrasonic test SYMBOL KEY_9 = 8 '[9] set Behaviour modes [9][9]=set US behaviour to User SYMBOL KEY_MINUS = 98 '[-] enable & test Whisker sensors and re-act, R_Whiskers: SYMBOL KEY_0 = 9 '[0] WANDER [9][0]=set vitality Behaviour to normal SYMBOL KEY_PLUS = 11 '[+] enable & test Ping sensor and re-act, R_Ping: '================================================================= ' INITIALISATION '========================== Init: hserptr =W2temp 'get back pointer SERTXD("{1",rootcmnd,cmnd,",") PWMOUT WhiskerLLED,255,10 'active high Dutycycle for obeying PWMOUT WhiskerRLED,255,10 'active high Dutycycle for obeying f8_doServos =$F0 'F=pulse servos, 0=done move f_MoveAborted =0 'AdjPosture: IF f_Bmodified=1 THEN GOSUB NewBehave '---------------------------------------------------- Qstanding: IF f_b_autoUp=1 THEN temp0 =senseLtoe +senseRtoe + senseLheel +senseRheel IF temp0=0 THEN GOSUB Qfell 'Check if fallen ENDIF 'Qfell: does GOSUB tsh, GOSUB tshh, GOSUB sshhh, while deciding if fallen over '==================================================== ' COMMANDER '==================================================== SELECT RootCmnd CASE "t" 'turn SELECT cmnd CASE "m" GOSUB Tm 'Turn Memory same way as last time CASE "o" GOSUB Tother 'Turn Otherway from last time CASE "a" GOSUB Ta 'Turn Away from leading foot CASE "t" GOSUB Tt 'Turn To leading foot ELSE GOSUB sshhh ENDSELECT CASE "a" 'actions SELECT cmnd CASE "1","b","2","e","3","o","4","v" RootCmnd ="v" GOTO Run2 'and here for IR CASE "5","S" GOSUB StampRL CASE "6","W" GOSUB WaveR CASE "7","K" GOSUB KickOut 'same as "K" CASE "8","V" GOSUB LVerticalSplits 'on Left foot CASE "9","u" GOSUB GetUp CASE "0","A" mindAt =mindstart 'same as "A" and here for IR CASE "+","E" cmnd ="E" :GOTO RUN2 'and here for IR ' CASE "F" GOSUB poseForwardsFull 'Baldur ' CASE "B" GOSUB poseBackFull 'Baldur ' CASE "f" GOSUB poseForwards 'Baldur ' CASE "b" GOSUB poseBack 'Baldur ' CASE "u" GOSUB poseUp 'Baldur ELSE GOSUB sshhh ENDSELECT CASE "u" 'up SELECT cmnd CASE "u","a" GOSUB GetUp 'auto get up CASE "f" GOSUB getUpF 'get up from on front CASE "b" GOSUB getUpB 'get up from on back ELSE GOSUB sshhh ENDSELECT 'u CASE "$" 'sense SELECT cmnd CASE "+" GOSUB QWhiskerFeelers :GOSUB NewMapWhiskersFeelers :GOSUB Robstacle 'from IR, react on CASE "-" GOSUB Qground :GOSUB Rhole 'from IR, react on CASE "0" f_b_react =0 :f_b_autosense =0 'for IR VCROSS="$" CASE "1" f_b_react =1 'for IR VCROSS="$" CASE "2","a" f_b_autosense =1 'for IR VCROSS="$" CASE "W" GOSUB QWhiskers :GOSUB NewMapWhiskers :GOSUB Robstacle CASE "F" GOSUB QFeelers :GOSUB NewMapFeelers :GOSUB Robstacle CASE "G" GOSUB Qground :GOSUB Rhole CASE "T" GOSUB QFell 'and do get up ELSE GOSUB sshhh ENDSELECT '$ CASE "?" SELECT cmnd CASE "W" GOSUB Qwhiskers : GOSUB NewMapWhiskers :GOTO Run0 '2023-01-03 CASE "F" GOSUB Qfeelers : GOSUB NewMapFeelers :GOTO Run0 '2023-01-03 ELSE GOSUB sshhh ENDSELECT ELSE SELECT cmnd CASE "F" GOSUB Fd CASE "B" GOSUB Bk CASE "R" GOSUB Rt CASE "L" GOSUB Lt CASE "K" GOSUB KickOut CASE "H" GOSUB Hlt ' CASE "V" GOSUB bip CASE "S" GOSUB StandStill : f8_doServos =0 'no pulsing in prog0 CASE "I" GOSUB InitLegs CASE "M" GOSUB Move 'used in Wait;, from CaseBehave to update values by NewBehave ELSE GOSUB sshhh ENDSELECT ENDSELECT 'don't Qtoes if only on one foot, should never happen with intrinsic Poses IF standOnLR=%11 AND f_b_autosense=1 THEN GOSUB Qground 'prints g,% IF sense0<>%1111 THEN GOSUB Autohole IF f_b_react=1 THEN GOSUB QWhiskerFeelers :GOSUB NewMapWhiskersFeelers :GOSUB AutoObstacle ENDIF ENDIF 'check for drop/object and ReaAct Run0: W2temp =hserptr 'Preserve pointer RUN 0 Run2: W2temp =hserptr 'Preserve pointer RUN 2 '==================================================== ' BEHAVIOUR '========================== NewBehave: 'SERTXD("NB") READ EEPROM_LHC,LHC LHC =LHC +TOD 'LHC set by 3-InitEEPROM:, 3-"b@" READ EEPROM_RHC,RHC RHC =RHC +TOD 'RHC set by 3-InitEEPROM:, 3-"b@" READ EEPROM_LAC,LAC LAC =LAC +TOF 'LAC set by 3-InitEEPROM:, 3-"b@" READ EEPROM_RAC,RAC RAC =RAC +TOF 'RAC set by 3-InitEEPROM:, 3-"b@" Sspeed =Scmnd -"0" MIN 1 'otherwise MOVE: hangs FL =FLcmnd -"1" *5 +16 '1-16 2-21 3-26 4-31 5-36 6-41 7-46 8-51 9-56 temp0 =Sspeed *2 +16 '1-18 2-20 3-22 4-24 5-26 6-28 7-30 8-32 9-34 FL =FL MIN temp0 FP =FPcmnd -"1" *2 +9 '1-9 2-11 3-13 4-15 5-17 6-19 7-21 8-23 9-25 FT =FTcmnd -"1" *2 +12 '1,12 2,14 3,16 4,18 5,20 6,22 7,24 8,26 9,28 f_Bmodified =0 RETURN '==================================================== ' MIND '========================== NewMapWhiskersFeelers: sense0 =0 MapWhiskersFeelers: GOSUB MapWhiskers GOSUB MapFeelers RETURN NewMapWhiskers: sense0 =0 MapWhiskers: PEEK RAM_Wthresh, temp55 PEEK RAM_whiskerL ,W2temp0 PEEK RAM_whiskerLlit,W2temp1 temp54 =W2temp0 MIN W2temp1 -W2temp1 IF temp54>temp55 THEN :bit1 =1 :HIGH whiskerLLED :ENDIF PEEK RAM_whiskerR, W2temp0 PEEK RAM_whiskerRlit,W2temp1 temp54 =W2temp0 MIN W2temp1 -W2temp1 IF temp54>temp55 THEN :bit0 =1 :HIGH whiskerRLED :ENDIF sertxd("WLR%",#bit1,#bit0,CR) ' %LR 1=>obstacles 0=>free '2023-01-03 RETURN NewMapFeelers: sense0 =0 MapFeelers: PEEK RAM_Fthresh, temp55 PEEK RAM_FeelerL, W2temp0 PEEK RAM_FeelerLlit,W2temp1 temp54 =W2temp0 MIN W2temp1 -W2temp1 IF temp54>temp55 THEN :bit1 =1 :HIGH feelerLLED :ENDIF PEEK RAM_FeelerR, W2temp0 PEEK RAM_FeelerRlit,W2temp1 temp54 =W2temp0 MIN W2temp1 -W2temp1 IF temp54>temp55 THEN :bit0 =1 :HIGH feelerRLED :ENDIF sertxd("FLR%",#bit1,#bit0,CR) ' %LR 1=>obstacles 0=>free '2023-01-03 RETURN '==================================================== ' SENSES '========================== QWhiskerFeelers: 'SYMBOL whiskerL =A.5 'READADC needs port.pin not a symbol 'SYMBOL whiskerR =A.6 'READADC needs port.pin not a symbol 'SYMBOL FrontLfeeler =D.6 'READADC needs port.pin not a symbol or pinX.Y 'SYMBOL FrontRfeeler =D.5 'READADC needs port.pin not a symbol or pinX.Y PWMOUT WhiskerLLED,OFF PWMOUT WhiskerRLED,OFF HIGH whiskerLLED HIGH whiskerRLED HIGH FeelerLLED HIGH FeelerRLED GOSUB ReadWhiskersFeelers POKE RAM_whiskerLlit,W2temp1 POKE RAM_whiskerRlit,W2temp0 POKE RAM_FeelerLlit,W27temp1 POKE RAM_FeelerRlit,W27temp0 LOW whiskerLLED LOW whiskerRLED LOW FeelerLLED LOW FeelerRLED GOSUB ReadWhiskersFeelers POKE RAM_whiskerL,W2temp1 POKE RAM_whiskerR,W2temp0 POKE RAM_FeelerL,W27temp1 POKE RAM_FeelerR,W27temp0 RETURN '---------------------------------------------------- ReadWhiskersFeelers: W2temp =0 W27temp =0 FOR cmnd =0 TO 6 READADC A.5,sense0 : W2temp1 =W2temp1 MIN sense0 READADC A.6,sense0 : W2temp0 =W2temp0 MIN sense0 READADC D.6,sense0 : W27temp1 =W27temp1 MIN sense0 READADC D.5,sense0 : W27temp0 =W27temp0 MIN sense0 NEXT cmnd RETURN '---------------------------------------------------- Qwhiskers: 'SYMBOL whiskerL =A.5 'READADC needs port.pin not a symbol 'SYMBOL whiskerR =A.6 'READADC needs port.pin not a symbol PWMOUT WhiskerLLED,OFF PWMOUT WhiskerRLED,OFF HIGH whiskerLLED HIGH whiskerRLED GOSUB ReadWhiskers POKE RAM_whiskerLlit,W2temp1 POKE RAM_whiskerRlit,W2temp0 sertxd(CR,"Wlit L",#W2temp1,",R",#W2temp0) '2023-01-03 LOW whiskerLLED LOW whiskerRLED GOSUB ReadWhiskers POKE RAM_whiskerL,W2temp1 POKE RAM_whiskerR,W2temp0 sertxd(CR,"Wdark L",#W2temp1,",R",#W2temp0,CR) '2023-01-03 RETURN '---------------------------------------------------- ReadWhiskers: W2temp =0 FOR cmnd =0 TO 6 READADC A.5,sense0 : W2temp1 =W2temp1 MIN sense0 'whiskerL READADC A.6,sense0 : W2temp0 =W2temp0 MIN sense0 'whiskerR next cmnd RETURN '---------------------------------------------------- QFeelers: 'SYMBOL FrontLfeeler =D.6 'READADC needs port.pin not a symbol or pinX.Y 'SYMBOL FrontRfeeler =D.5 'READADC needs port.pin not a symbol or pinX.Y HIGH FeelerLLED HIGH FeelerRLED GOSUB ReadFeelers POKE RAM_FeelerLlit,W2temp1 POKE RAM_FeelerRlit,W2temp0 sertxd(CR,"Flit L",#W2temp1,",R",#W2temp0) '2023-01-03 LOW FeelerLLED LOW FeelerRLED GOSUB ReadFeelers POKE RAM_FeelerL,W2temp1 POKE RAM_FeelerR,W2temp0 sertxd(CR,"Fdark L",#W2temp1,",R",#W2temp0,CR) '2023-01-03 RETURN '---------------------------------------------------- ReadFeelers: W2temp =0 for cmnd =0 to 6 READADC D.6,sense0 : W2temp1 =W2temp1 MIN sense0 'feelerL READADC D.5,sense0 : W2temp0 =W2temp0 MIN sense0 'feelerR next cmnd RETURN '---------------------------------------------------- Qground: '-> sense0 'Q on Ground=1 "$G", auto if f_b_Qground=1 sertxd("Qg") sense0 =0 FOR temp55 =0 TO 25 bit1 =bit1 OR senseLtoe bit0 =bit0 OR senseRtoe bit2 =bit2 OR senseRheel 'for hole going Bk bit3 =bit3 OR senseLheel 'for hole going Bk IF sense0=%1111 THEN EXIT PAUSE 8 '4ms @m16 max about 0.1 sec NEXT 'temp55 RETURN '---------------------------------------------------- Qfell: 'sertxd("Qfell") 'make sure really tipped before getting up so test three times FOR temp55=0 TO 1 READADC A.7,temp0 'A_tipFB, READADC needs port.pin not a symbol 'sertxd(#temp0,"F",#TipF,"U",#TipU) IF temp0TipU THEN Sspeed =Sspeed MIN 2 -1 GOSUB sshhh PAUSE 100 ELSE RETURN ENDIF NEXT IF f_b_react=0 THEN RETURN ENDIF '---------------------- GetUp: READADC A.7,temp0 'A_tipFB, READADC needs port.pin not a symbol SERTXD(#temp0,"F",#TipF,"U",#TipU) IF temp0TipU THEN Sspeed =Sspeed MIN 2 -1 SELECT temp0 CASE TipB GOTO getUpB 'TipB threshold = 200 CASE >TipU GOTO getUpU 'TipU threshold = 120 ENDSELECT ENDIF RETURN '==================================================== ' REACTIONS '========================== Robstacle: 'sense0 set in NewMapWhiskersFeelers: SERTXD("ro") IF f_b_react=0 THEN SERTXD("q") RETURN ENDIF AutoObstacle: IF sense0=%00 THEN RETURN ENDIF '2023-01-03 sertxd(" obstacle",CR) ' %LR 1=>obstacles 0=>free '2023-01-03 IF sense0=%11 THEN :ReactAt =OLR :ENDIF IF sense0=%10 THEN :ReactAt =OL_ :ENDIF IF sense0=%01 THEN :ReactAt =O_R :ENDIF RETURN '---------------------------------------------------- Rhole: 'sense0 set in Qground: SERTXD("rg") IF f_b_react=0 THEN SERTXD("q") RETURN ENDIF SERTXD("%",#bit3,#bit2,#bit1,#bit0) ' %LR 0=>holes 1=>on ground Autohole: GOSUB Bip GOSUB Qground IF sense0<>%1111 THEN GOSUB Kickout GOSUB Qground IF sense0=%1111 THEN RETURN :ENDIF GOSUB Qground IF sense0<>%1111 THEN GOSUB Kickout GOSUB Qground IF sense0=%1111 THEN RETURN :ENDIF IF f_b_F=0 THEN _avoidback _avoidfront: sertxd("f") sense0 =sense0 & %11 IF sense0=%00 THEN :ReactAt =HLR :ENDIF '2023-01-03 IF sense0=%01 THEN :ReactAt =HL_ :ENDIF '2023-01-03 IF sense0=%10 THEN :ReactAt =H_R :ENDIF '2023-01-03 RETURN _avoidback: sertxd("b") sense0 =sense0 /4 IF sense0=%00 THEN :ReactAt =HBLR :ENDIF '2023-01-03 IF sense0=%01 THEN :ReactAt =HBL_ :ENDIF '2023-01-03 IF sense0=%10 THEN :ReactAt =HB_R :ENDIF '2023-01-03 RETURN '==================================================== ' ACTS '========================== WaveR: GOSUB PH_SL 'PT-BB PT_FF are full back and full forward - only for getting up GOSUB PT_0F GOSUB PT_0B GOSUB PT_0F GOSUB PT_0B GOSUB PH_SLR RETURN LVerticalSplits: 'do vertical splits on R foot standOnLR=%01 'sertxd("WaveR") Sspeed =Sspeed MAX 2 RHto =RHmid +35 GOSUB Move RAto =RAmid LAto =LAmid GOSUB Move RHto =140 '@about80deg - more and overbalances LHto =LHmid hframes =12 'about 1/2 sec GOSUB Move LHto =LHout '@90deg GOSUB Move RAto =RAmid +30 'wave RA hframes =6 'about 1/4 sec GOSUB Move RAto =RAmid -30 'wave RA GOSUB Move RAto =RAmid GOSUB Move Sspeed =8 LAto =LAmid +60 'wave LA GOSUB Move LAto =LAmid -60 'wave LA GOSUB Move Sspeed =2 GOSUB PT_HH GOSUB PH_SLR 'sort of fall back onto feet f_Bmodified =1 RETURN '==================================================== ' ACTIONS '========================== InitLegs: sertxd("initlegs") Sframe =30 'do it slowly RHto =RHC 'centre Servo Right Hip LHto =LHC 'centre Servo Left Hip FOR cmnd =0 TO 50 '50=some number 'Hips@=mid => feet flat on ground GOSUB TellHipServos 'don't pulse ankles NEXT temp0 =RHC +FL +FL 'foot lift twice normal FOR cmnd =0 TO 30 '30=some number 'stand on Right Foot LHat =LHat -1 MIN LHinmin RHat =RHat +1 MAX temp0 GOSUB TellHipServos 'don't pulse ankles NEXT RAto =RAFWmid LAto =LABWmid GOSUB Move 'pulse all 'left leg lower to stand on both feet LHat =LHat +1 MAX LHC 'left leg out, RHat =RHat -1 MIN RHC 'right leg in GOSUB Move'TellServos f_Rfwd =1 Sframe =DSframe 'LOOPs at ~40Hz/25ms set back to default RETURN '---------------------------------------------------- getUpF: 'get up from on front SERTXD("LieUF ") standOnLR=%00 GOSUB PT_BB RHto =RHout +35 LHto =LHout +35 hframes =10 GOSUB MOVE GOSUB PT_MS GOSUB PH_SLR GOSUB PT_BF RETURN '------------- getUpB: 'get up from on back SERTXD("LieUB ") standOnLR=%00 GOSUB PT_FF RHto =RHout +35 LHto =LHout +35 hframes =10 GOSUB MOVE GOSUB PT_MS GOSUB PH_SLR GOSUB PT_BF RETURN '------------- getUpU: 'get up from upside down SERTXD("LieUU ") standOnLR=%00 GOSUB PT_FF RHto =RHout +55 LHto =LHout +55 hframes =50 GOSUB MOVE RAto =RAmin hframes =50 GOSUB MOVE LAto =LAmin hframes =50 GOSUB MOVE GOSUB PT_MS GOSUB PH_SLR GOSUB PT_BF RETURN '---------------------------------------------------- #rem KickL: hframes =10 ' GOSUB P_SR Sspeed =12 RHto =RHout +40 LHto =LHout +40 GOSUB MOVE RHto =RHout -30 GOSUB MOVE RHto =RHout hframes =10 GOSUB MOVE GOSUB PH_SLR f_Bmodified =1 RETURN #endrem '---------------------------------------------------- StampRL: 'a4, ^4 'sertxd("StampRL") GOSUB StampR GOSUB StampL RETURN '----------------------- StampR: 'sertxd("StampR") hframes =12 'about 1/2 sec GOSUB PH_SL Sspeed =Sspeed +3 MAX 8 hframes =12 'about 1/2 sec GOSUB PH_SLR f_Bmodified =1 RETURN '----------------------- StampL: 'sertxd("StampL") hframes =12 'about 1/2 sec GOSUB PH_SR Sspeed =Sspeed +3 MAX 8 hframes =12 'about 1/2 sec GOSUB PH_SLR f_Bmodified =1 RETURN '----------------------- KickOut: 'a6, ^6, K sertxd("k",kickcmnd) temp0 =kickcmnd -"0" *8 LHto =LHmid +temp0 RHto =RHmid +temp0 standOnLR=%00 GOSUB Move GOSUB PH_SLR RETURN '==================================================== MOVEMENT: '========================== Hlt: 'stand with right foot forward IF f_Rfwd =1 THEN RETURN ENDIF IF f_b_F =1 THEN GOSUB Fd 'make Rfwd ELSE GOSUB Bk 'make Rfwd ENDIF RETURN '----------------------- StandStill: 'Stand Still 'NEEDS WORK - needs foot switches for stood on foot IF RHat=RHC AND LHat=LHC THEN RETURN ENDIF W2pulselo =RHat-RHC W2pulsehi =LHat-LHC IF W2pulselo>W2pulsehi THEN RHto =RHC +FL +FL LHto =LHC ELSE LHto =LHC +FL +FL RHto =RHC ENDIF GOSUB MOVE GOSUB PT_HH GOSUB PH_SLR f_Rfwd =1 RETURN '----------------------- Fd: 'SERTXD("Fd") IF f_Rfwd =1 THEN GOSUB PH_SR GOSUB PT_FB GOSUB PH_SLR f_Rfwd =0 ELSE GOSUB PH_SL GOSUB PT_BF GOSUB PH_SLR f_Rfwd =1 ENDIF f_b_F =1 RETURN '----------------------- Bk: SERTXD("B") IF f_Rfwd =1 THEN GOSUB PH_SL 'SERTXD(" LHat=",#LHat," LAat=",#LAat," RHat=",#RHat," RAat=",#RAat,CR) GOSUB PT_FB 'SERTXD(" LHat=",#LHat," LAat=",#LAat," RHat=",#RHat," RAat=",#RAat,CR) GOSUB PH_SLR 'SERTXD(" LHat=",#LHat," LAat=",#LAat," RHat=",#RHat," RAat=",#RAat,CR) f_Rfwd =0 ELSE GOSUB PH_SR GOSUB PT_BF GOSUB PH_SLR f_Rfwd =1 ENDIF f_b_F =0 RETURN '----------------------- Tm: 'TurnMemory: PEEK RAM_P1_f8_flags,sense0 'see which turn was last IF bit0=0 THEN Rt GOTO Lt RETURN 'removing redundant RETURN increases bytes!!!! '----------------------- Tother: 'TurnOther: PEEK RAM_P1_f8_flags,sense0 'see which turn was last IF bit0=0 THEN Lt GOTO Rt RETURN 'removing redundant RETURN increases bytes!!!! '----------------------- Tt: 'TurnTo IF f_Rfwd=0 THEN Lt GOTO Rt '----------------------- Ta: 'TurnAway: IF f_Rfwd=1 THEN Lt 'else drop through to Rt '----------------------- Rt: 'SERTXD("R") IF f_b_F=1 THEN 'forwards IF f_Rfwd=1 THEN GOSUB PH_SR GOSUB PT_0B f_Rfwd =0 ELSE GOSUB PH_SL GOSUB PT_0F f_Rfwd =1 ENDIF ELSE 'backwards IF f_Rfwd=1 THEN GOSUB PH_SL GOSUB PT_F0 f_Rfwd =0 ELSE GOSUB PH_SR GOSUB PT_B0 f_Rfwd =1 ENDIF ENDIF GOSUB PH_SLR 'remember turn PEEK RAM_P1_f8_flags,sense0 :bit0 =0 :POKE RAM_P1_f8_flags,sense0 RETURN '----------------------- Lt: 'SERTXD("L") IF f_b_F=1 THEN 'forwards IF f_Rfwd=0 THEN GOSUB PH_SL GOSUB PT_B0 f_Rfwd =1 ELSE GOSUB PH_SR GOSUB PT_F0 f_Rfwd =0 ENDIF ELSE 'backwards IF f_Rfwd=0 THEN GOSUB PH_SR GOSUB PT_0F f_Rfwd =1 ELSE GOSUB PH_SL GOSUB PT_0B f_Rfwd =0 ENDIF ENDIF GOSUB PH_SLR 'remember turn PEEK RAM_P1_f8_flags,sense0 :bit0 =1 :POKE RAM_P1_f8_flags,sense0 RETURN '==================================================== ' POSE xH_xx Hip, xT_xx Toe '========================== PH_SLR: 'Stand on Left + Right 'SERTXD("SLR ") LHto =LHC RHto =RHC standOnLR =%11 GOTO MOVE PH_SR: 'Stand on Right 'SERTXD("SR ") LHto =LHC MIN FL -FL MIN LHinmin RHto =RHC +FL MAX 255 standOnLR =%01 GOTO MOVE PH_SL: 'Stand on Left 'SERTXD("SL FL=",#FL) LHto =LHC +FL MAX 255 'adding, say, 5 to LHto makes Rlift = Llift, but Lfoot bounces RHto =RHC MIN FL -FL MIN RHinmin standOnLR =%10 GOTO MOVE '----------------------- 'Toes (left)(right) 0=Mid, F=Forward, B=Backward PT_0F: LAto =LAC RAto =RAC +FT MAX 255 GOTO MOVE PT_F0: LAto =LAC +FT MAX 255 RAto =RAC GOTO MOVE PT_0B: LAto =LAC RAto =RAC MIN FT -FT GOTO MOVE PT_B0: LAto =LAC MIN FT -FT RAto =RAC GOTO MOVE '----------------------- PT_BF: LAto =LAC MIN FP -FP RAto =RAC +FP MAX 255 GOTO MOVE PT_FB: LAto =LAC +FP MAX 255 RAto =RAC MIN FP -FP GOTO MOVE PT_HH: 'Walk Toes Halt LAto =LABWmid RAto =RAFWmid GOTO MOVE '----------------------- PT_FF: 'getUpB: LAto =LAmid +85 MAX 255 RAto =RAmid +85 MAX 255 GOTO MOVE PT_MS: 'getUpU: 'Toe Mid Stand - toes offset LAto =LAC -TSO RAto =RAC +TSO GOTO MOVE PT_BB: 'getUpF: LAto =LAmid -85 MIN RAmin RAto =RAmid -85 MIN RAmin GOTO MOVE '==================================================== ' HMI '========================== ' SOUND voice,(sound,time) time=~2.5ms @16MHz bip: SOUND voice,(80,7) RETURN '----------------------- sshhh: 'because that is what it sounds like 'beep255: SOUND voice,(240,80) '(255,10ms) @32MHz RETURN '----------------------- 'tshh: 'because that is what it sounds like ' SOUND voice,(240,20) '(255,10ms) @32MHz ' 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 LHmin MAX LHmax LAto =LAto MIN LAmin MAX LAmax RHto =RHto MIN RHmin MAX RHmax RAto =RAto MIN RAmin MAX RAmax topple =0 '---------------------------------------------------- MoveCalcs: 'sertxd(#f_LAdo, #f_LHdo, #f_RHdo, #f_RAdo,cr) 'sertxd("AtTo",#LHat," ",#LHto," ",#RHat," ",#RHto," ",#Sspeed) '------------- _RA: 'Right Ankle 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 stance toe off ground IF f_b_autosense=1 THEN IF standOnLR=%10 THEN IF senseLtoe=0 AND senseLheel=0 THEN : topple =topple +1 :ENDIF ENDIF IF standOnLR=%01 THEN IF senseRtoe=0 AND senseRheel=0 THEN : topple =topple +1 :ENDIF ENDIF IF topple>25 THEN AbortMove 'on faster speeds bounce never gets to 5 ENDIF 'f_b_Qground W2pulselo =f_LAdo +f_LHdo +f_RHdo +f_RAdo IF W2pulselo<>0 THEN MoveCalcs hframes =hframes MIN 1 -1 IF hframes<>0 THEN MoveCalcs IF f_MoveAborted=1 THEN Run0 'quit Intrinsic Act now done AbortMove RETURN AbortMove: ' toe 0=off ground -> falling? so abortMove and do SLR sertxd(#topple,"abort",cr) Sspeed =9 :f_Bmodified =1 : f_MoveAborted =1 : f_LApulse =0: f_RApulse =0 GOTO PH_SLR '====================================================