'Ullr_8-1.bas 'Ullr_7-1.bas 40x2 Firmware v3 #picaxe 40x2 #slot 1 '#no_table #no_data #define keybip 'for GetCmnd DISCONNECT SETFREQ m16 'SERTXD 19200 @m16 'HSERSETUP B19200_16,%00101 'pulsout 4MHz 10us units 'pulsout 16MHz 2.5us units '#1 Memory used = 4052 bytes out of 4096 #rem ============================================ PROG1 ----- Init: Qfell: SELECT cmnd -------- ' SENSES Qping: Qfeelers: Qwhiskers: Qtoefeelers: Qground: Qfell: Qtoes:'->sense0 'Q on Ground=1 "G", auto if f_bg_Qtoes=1 -------- ' BEHAVIOUR NewBehave: Rhole: -------- ' ACTS WaveL: -------- ' ACTIONS InitLegs: getUpF: 'get up from on front getUpB: 'get up from on back getUpHead: getUpU: 'get up from upside down StampRL: StampR: StampL: KickOut: liedown: standup: -------- ' MOVEMENT: Fd: Bk: Rt: Lt: TurnR: TurnL: Hlt: 'stand with right foot forward StandStill: 'Stand Still 'NEEDS WORK - needs foot switches for stood on foot crawlForwards: crawlBack: crawlRight: crawlLeft: -------- ' POSE poseForwards: poseBack: poseUp: poselie: PR_SLR: 'Stand on Left + Right PR_SR: 'Stand on Right PR_SL: 'Stand on Left PP_WBF: 'Walk BF PP_WFB: 'Walk FB PP_HH: 'Pose HH Toes Halt PP_MS: 'Pose MS 'Toe Mid Stand - toes offset PP_FF: 'Pose FF PP_BB: 'Pose BB -------- ' HMI bip: beep50: tsh: 'because that is what it sounds like tshh: 'because that is what it sounds like sshhh: 'because that is what it sounds like beep255 whee: 'make rising sound whoa: 'make falling sound: SETUP: 'Can't do this by IRremote so don't test IR GetCmnd: Qserial: MOVE: MoveCalcs: TellServos: '@16MHz ----------------------------- PROG2 ----- Init: DumpActs: 'Dump sends all DATA to PC DumpASCIIacts: PrintHelp: sshhh: 'because that is what it sounds like #endrem '************************* Machine Dependent ***************************** 'Notes '----- 'Servos ' '************************* END Machine Dependent ************************* '================================================== Pins: '================================================== SYMBOL servoLR =A.1 SYMBOL servoLP =A.0 SYMBOL servoRR =D.3 SYMBOL servoRP =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 '================================================== '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 HRL, ("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 ReactEnd =127 'TABLE ReactEnd,(".") '================================================== VarsCons: '========================== SYMBOL mindstart =128 'table in #0 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 ground sensors, "bg" 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 '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_RRdo =bit24 '-!-! do servo SYMBOL f_RPdo =bit25 '-!-! do servo SYMBOL f_LRdo =bit26 '-!-! do servo SYMBOL f_LPdo =bit27 '-!-! do servo SYMBOL f_RRpulse =bit28 'r!-r pulse servos SYMBOL f_RPpulse =bit29 'r!-r pulse servos SYMBOL f_LRpulse =bit30 'r!-r pulse servos SYMBOL f_LPpulse =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 'W4 SYMBOL RPC =b8 'Right Hip Pace Centre for current posture SYMBOL LPC =b9 'Left Hip Pace Centre for current posture 'W5 SYMBOL RRC =b10 'Right Hip Roll Centre for current posture SYMBOL LRC =b11 'Left Hip Roll Centre for current posture 'W6 SYMBOL RPat =b12 'Servo Right Hip Pace At SYMBOL RPto =b13 'Servo Right Hip Pace To 'W7 SYMBOL LPat =b14 'Servo Left Hip Pace At SYMBOL LPto =b15 'Servo Left Hip Pace To 'W8 SYMBOL RRat =b16 'Servo Right Hip Roll At SYMBOL RRto =b17 'Servo Right Hip Roll To 'W9 SYMBOL LRat =b18 'Servo Left Hip Roll At SYMBOL LRto =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 FinCmnd =b36 'Foot In = 9 -FUI *FL /9 SYMBOL ReactAt =b37 'React Table pointer '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 RHcmnd =b44 'controls Hip Centres for RideHeight 0-9 in 1-NewBehave: 'Thor ''SYMBOL RHcmndold =b45 'last Ride height cmnd 'Thor 'W23 'W24 SYMBOL mindAt =b48 ' SYMBOL mindSub =b49 ' 'W25 'W26 'SYMBOL stackcmnd =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_leglLift =79 SYMBOL RAM_FootIn =80 'SYMBOL RAM_randvarLo =68 'SYMBOL RAM_randvarHi =69 'SYMBOL RAM_randseedlo =70 'SYMBOL RAM_randseedhi =71 SYMBOL EEPROM_LPC =255 'logical value SYMBOL EEPROM_LRC =254 'logical value SYMBOL EEPROM_RPC =253 'logical value SYMBOL EEPROM_RRC =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_FinCmnd =246 '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 @ - values are in 10us units 'Cirrus-CS601-BB-STD 5.5Kgcm@4.8v, 0.2sec, +ve clockwise, 'jerk on poweron if not already being pulsed. '================================================== SYMBOL SLPback = 47 '106 SYMBOL SLPb90 = 61 ' 92 SYMBOL SLPmid =153 ' 0 SYMBOL SLPupmax =225 'otherwise legs hit face SYMBOL SLPf90 =246 ' 93 SYMBOL SLPfwd =250 ' 97 SYMBOL SLRout = 56 '140 SYMBOL SLRo90 =102 ' 95 SYMBOL SLRoeasy =186 ' 10 SYMBOL SLRoattn =191 ' 5 SYMBOL SLRstand =196 ' 0 SYMBOL SLRin =237 ' 41 SYMBOL SRPback =250 ' 96 SYMBOL SRPb90 =250 ' 96 SYMBOL SRPmid =154 ' 0 SYMBOL SLRupmax =172 'otherwise legs hit face SYMBOL SRPf90 = 61 ' 93 SYMBOL SRPfwd = 53 '101 SYMBOL SRRout =234 '127 'seems to be stopped by something SYMBOL SRRo90 =198 ' 91 SYMBOL SRRoeasy =117 ' 10 SYMBOL SRRoattn =112 ' 5 SYMBOL SRRstand =107 ' 0 SYMBOL SRRin = 68 ' 39 'Left foot forward is +ve 'Left leg out is -ve 'Right foot forward is -ve 'Right leg out is +ve '================================================== 'LOGICAL 'foot forward(toe in) is +ve - if Lfd,Rbk > left foot is in front on the ground 'leg out is +ve SYMBOL LPback =SLPback SYMBOL LPb90 =SLPb90 SYMBOL LPmid =SLPmid SYMBOL LPf90 =SLPf90 SYMBOL LPfwd =SLPfwd SYMBOL LRomax =256 -SLRout SYMBOL LRo90 =256 -SLRo90 SYMBOL LRlie =LRo90 SYMBOL LRoeasy =256 -SLRoeasy SYMBOL LRoattn =256 -SLRoattn SYMBOL LRstand =256 -SLRstand SYMBOL LRin =256 -SLRin SYMBOL RPback =256 -SRPback SYMBOL RPb90 =256 -SRPb90 SYMBOL RPmid =256 -SRPmid SYMBOL RPf90 =256 -SRPf90 SYMBOL RPfwd =256 -SRPfwd SYMBOL RRomax =SRRout SYMBOL RRo90 =SRRo90 SYMBOL RRlie =RRo90 SYMBOL RRoeasy =SRRoeasy SYMBOL RRoattn =SRRoattn SYMBOL RRstand =SRRstand SYMBOL RRin =SRRin 'for servo numbering in CaseAdjust: SYMBOL LP =3 SYMBOL LR =2 SYMBOL RP =1 SYMBOL RR =0 '-------------------------------------------------------------- SYMBOL DSframe =15 'default SFrame, LOOPs at ~40Hz/25ms SYMBOL Fclear =20 'minimum for toes to miss when walking SYMBOL TSO =3 'Toe Stand Offset - so toes clear each other '================================================== ' SENSORS '========================== PingSensors: '----------- ' At sea level sound travels through air at 1130 feet per second. ' This equates to 1 inch in 73.746 uS, or 1 cm in 29.034 uS. ' ' Since the Ping sensor measures the time required for the sound wave ' to travel from the sensor and back. The result is divided by two to ' remove the return portion of the echo pulse, then multiplied by 10 ' to convert 10uS resolution to uS. ' The final raw result is converted to cm.or inches ' uS * 10/2/73.746in => * 0.01356 => * 4443/65536 => ** 4443 ' uS * 10/2/29.034cm => * 0.03444 => * 11286/65536 => ** 11286 'SYMBOL uS10toinch =4443 ' 10 / 58.068 (with **) 'SYMBOL uS10tocm =11286 ' 10 / 147.492 (with **) SYMBOL uS10tocm4mHz =11286 ' 10 / 147.492 (with **) SYMBOL cmtoinch =25802 ' /2.54 -> *25802 /65536 -> **25802 'PULSIN resolution @16MHz=2.5uS SYMBOL USscalecm =uS10tocm4mHz /4 'pulsin units 2.5uS @16MHz SYMBOL pingtrig =6 '15uS @16Mhz 'SRF05 'If no echo SRF05 ultrasonic sensor times out at 32ms 'HC-SR04 'If no echo HC-SR04 ultrasonic sensor never times out. 'The HC-SR04 starts the return pulse too quickly for a PicAxe20m2 @4Mhz 'to detect the start of the pulse. It needs to run at least @8MHz. 'Also if the sensor never gets a return echo from an obstacle the 'return pulse never ends and the PULSIN timer has to overflow before 'it returns a value. At 8MHz the timer resolution is 5us. The timer 'is 16 bits which means each reading takes '65,536 * 5us =327680 'which is about 1/3 of a second. This is a long time to wait between 'steps and makes walking very slow. 'Increasing the clock speed to 16MHz means the timer resolution is '2.5us and it times out in 65536 *2.5us = 163990us or about 0.16 'of a second which is much better. 'So SETFREQ m16 should be used (max for 40x2) '================================================================= ' INITIALISATION '========================== Init: hserptr =W2temp 'get back pointer SERTXD("{1",rootcmnd,cmnd,",") 'Scmnd",Scmnd," Sspeed",#Sspeed,",") PWMOUT EyeLLED,255,100 'active high Dutycycle for obeying PWMOUT EyeRLED,255,100 'active high Dutycycle for obeying f8_doServos =$F0 'F=pulse servos, 0=done move 'AdjPosture: IF f_Bmodified=1 THEN GOSUB NewBehave '---------------------------------------------------- Qstanding: ' IF standOnLR=%11 AND f_b_Qground=1 THEN ' IF senseLtoe=0 THEN GOSUB KickOut ' IF senseRtoe=0 THEN GOSUB KickOut IF f_b_autoUp=1 THEN GOSUB Qfell '==================================================== ' COMMANDER '==================================================== SELECT RootCmnd 'CaseTurn: CASE "t" 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 CASE "r" GOSUB poseRskid 'skid turn CASE "l" GOSUB poseLskid 'skid turn ELSE GOSUB sshhh ENDSELECT 'CaseActions: CASE "a" SELECT cmnd CASE "1","b","2","e","3","o","4","v" RootCmnd ="v" GOTO Run2 'and here for IR 'CASE "1","b" GOSUB bip 'and here for IR 'CASE "2","e" GOSUB whee 'and here for IR 'CASE "3","o" GOSUB whoa 'and here for IR CASE "5","S" GOSUB StampRL CASE "6","W" GOSUB WaveL CASE "7","K" GOSUB KickOut 'and here for IR CASE "8","s" GOSUB poseSit :f_Crawl =1 :f8_doServos =0 'no pulsing in prog0 CASE "9","u" GOSUB standUp :f_Crawl =0 :f8_doServos =0 'no pulsing in prog0 (Roeasy PC|TOS) CASE "0","A" mindAt =mindstart 'same as "A" and here for IR CASE "+","E" cmnd ="E" :GOTO RUN2 'and here for IR CASE "R" GOSUB stepR 'stepR here for IR ">" CASE "L" GOSUB stepL 'stepL here for IR "<" CASE ">" GOSUB kickR ' CASE "<" GOSUB kickL ' ELSE GOSUB sshhh ENDSELECT 'CasePose: CASE "p" SELECT cmnd CASE "u" GOSUB poseUp 'at FL CASE "e" GOSUB poseEasy :f8_doServos =0 'no pulsing in prog0 CASE "s" GOSUB poseSit :f8_doServos =0 'no pulsing in prog0 CASE "f" GOSUB poseForwards CASE "m" GOSUB poseMid CASE "b" GOSUB poseBack CASE "F" GOSUB poseForwardsFull CASE "B" GOSUB poseBackFull ELSE GOSUB sshhh ENDSELECT CASE "u" 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 'CASE "h","B" GOSUB getUpHeadB 'get up from on head 'CASE "F" GOSUB getUpHeadF 'get up from on head ELSE GOSUB sshhh ENDSELECT 'u CASE "$" SELECT cmnd CASE "+" GOSUB QWhiskerFeelers :GOSUB NewMapWhiskersFeelers :GOSUB Robstacle 'for IR, react on CASE "-" GOSUB Qground :GOSUB Rhole 'for IR, auto & 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 "3" f_b_Uon =0 'for IR VCROSS="$" CASE "4" f_b_Uon =1 'for IR VCROSS="$" CASE "U" GOSUB Qping :GOSUB Robject 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 "c" SELECT cmnd CASE "f" GOSUB crawlForwards CASE "b" GOSUB crawlBack CASE "r" GOSUB crawlRight CASE "l" GOSUB crawlLeft ELSE GOSUB sshhh ENDSELECT CASE "?" SELECT cmnd CASE "U" GOSUB Qping : GOTO Run0 '2023-01-03 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 StandEasy :f_Crawl=0 CASE "<" GOSUB stepL CASE ">" GOSUB stepR CASE "I" GOSUB InitLegs CASE "M" GOSUB Move 'used in Wait; also from LoadBehaviour: and 'from CaseBehave to update values by NewBehave: 'could have used say "N" and gosub 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 'check for drop and ReaAct GOSUB Qground 'prints % sets sense0 from toes IF sense0<>3 THEN GOSUB Autohole IF f_b_react=1 THEN GOSUB QWhiskerFeelers :GOSUB NewMapWhiskersFeelers :GOSUB AutoObstacle ENDIF IF f_b_Uon=1 THEN GOSUB Qping :GOSUB AutoObject 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("newbehave Scmnd",Scmnd," ") READ EEPROM_RPC,RPC :RPC =RPC -TOO 'RPC set in @ READ EEPROM_LPC,LPC :LPC =LPC -TOO 'LPC set in @ READ EEPROM_RRC,RRC 'RRC set in @ READ EEPROM_LRC,LRC 'LRC set in @ Sspeed =Scmnd -"0" MIN 1 'otherwise MOVE: hangs POKE RAM_Sspeed,Sspeed temp0 =FinCmnd -"0" temp0 =RRstand -RRin +LRstand -LRin /2 *temp0 /9 'average of In scaled by Fin POKE RAM_FootIn,temp0 temp0 =FLcmnd MIN FinCmnd 'otherwise toes hit FL =temp0 -"0" *10' +19 '0=19 1=24 2=29 3=34 4=39 5=44 6=49 7=54 8=59 9=64 POKE RAM_leglLift,FL ' temp0 =Sspeed *2 +10 '1=12 2=14 3=16 4=18 5=20 6=22 7=24 8=26 9=28 ' FL =FL MIN temp0 ' FP =FPcmnd -"0" *2 +2 '0=2 1=4 2=6 3=8 4=10 5=12 6=14 7=16 8=18 9=20 <38 FP =FPcmnd -"0" *4 +2 '0=2 1=6 2=10 3=14 4=18 5=22 6=26 7=30 8=34 9=38 <38 FT =FTcmnd -"0" *3 +2 '0=2 1=5 2=8 3=11 4=14 5=17 6=20 7=23 8=26 9=29 <38 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 'full dark =255 PEEK RAM_whiskerLlit,W2temp1 'full lit =0 temp54 =W2temp0 MIN W2temp1 -W2temp1 IF temp54>temp55 THEN :bit1 =1 :HIGH whiskerLLED :ENDIF PEEK RAM_whiskerR, W2temp0 'full dark =255 PEEK RAM_whiskerRlit,W2temp1 'full lit =0 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 'full dark =255 PEEK RAM_FeelerLlit,W2temp1 'full lit =0 temp54 =W2temp0 MIN W2temp1 -W2temp1 IF temp54>temp55 THEN :bit1 =1 :HIGH feelerLLED :ENDIF PEEK RAM_FeelerR ,W2temp0 'full dark =255 PEEK RAM_FeelerRlit,W2temp1 'full lit =0 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 '========================== '---------------------------------------- #rem setUSawares: SELECT subcmnd CASE "m" GOSUB setUSawares_m 'set thresholds manually CASE "+" GOSUB QRecordGetCmnd SELECT cmnd CASE "0" TO "9" ' temp =cmnd -"0" :temp =temp *2 ' PEEK RAMUSunits_cm,i_temp ' IF i_temp=1 THEN :temp =temp *5 /2 ENDIF temp =cmnd -"0" *5 'units cm POKE RAMUSawareAt,temp ENDSELECT CASE "-" GOSUB QRecordGetCmnd SELECT cmnd CASE "0" TO "9" ' temp =cmnd -"0" :temp =temp *2 ' PEEK RAMUSunits_cm,i_temp ' IF i_temp=1 THEN :temp =temp *5 /2 ENDIF temp =cmnd -"0" *5 'units cm POKE RAMUStoonear,temp ENDSELECT CASE "i" POKE RAMUSunits_cm_,1 'inches CASE "c" POKE RAMUSunits_cm_,0 'cm ELSE GOSUB sshhh ENDSELECT RETURN '---------------------------------------- setUSawares_m: IF f_IRcmnd=1 THEN IRIN inIR,cmnd 'setup entered from IRremote ELSE GOSUB GetCmnd ENDIF GOSUB beep50 'got cmnd W2temp =0 _getUSdist: GOSUB Qping SERTXD("US",#usdist,CR) IF usdist=0 THEN GOSUB badkey GOTO _getUSdist 'loop until valid reading ENDIF 'SERTXD("cmnd",#cmnd,CR) SELECT cmnd CASE "+",KEY_PLUS POKE RAMUSawareAt,usdist 'SERTXD("KEY_PLUS",CR) 'SERTXD("AwareAt=",#usdist,CR) CASE "-",KEY_MINUS POKE RAMUStoonear,usdist 'SERTXD("KEY_MINUS",CR) 'SERTXD("TooClose=",#usdist,CR) CASE "Q",KEY_POWER,Esc 'Quit GOSUB beep50 'done 'SERTXD("KEY_POWER",CR) RETURN 'RETURN ELSE GOSUB badkey ENDSELECT GOTO setUSawares_m #endrem '----------------- Qping: 'Query Ultrasonic Sensor, read into USdist POKE RAM_USlast,usdist LOW pingpin ' make trigger 0-1-0 PULSOUT pingpin,pingtrig ' activate sensor PULSIN pingpin,1,W2temp ' measure high(=1) echo pulse in units, ' units are 10us@4MHz, 2.5us@32MHz ' pulsin rolls over at 65536counts =>0 ' ie 0.65536sec @ 4MHz(10uS) too long to wait if nothing there ' 0.16399sec @16MHz(2.5uS) tolerable ' so use 16MHz for HCSR04 ' SRF05 auto times out at 36ms and returns 0, good. ' PEEK RAMUSunits_cm_,temp ' IF temp=0 THEN W2temp =W2temp ** USscalecm 'scale time to cm ' ELSE ' W2temp =W2temp ** cmtoinch ' ENDIF IF W2temp=0 THEN ' if timeout then usdist =255 ' set to max distance ELSE usdist =W2temp MAX 255 ENDIF SERTXD("U(",#usdist,"cm)") usdist =usdist /5 'convert to 5cm/2inch units SERTXD(#usdist,CR) ' %LR 0=>obstacles 1=>free RETURN '---------------------------------------------------- QWhiskerFeelers: 'SYMBOL whiskerL =A.5 'READADC needs port.pin not a symbol 'SYMBOL whiskerR =D.0 'READADC needs port.pin not a symbol 'SYMBOL toeLfeeler =D.6 'READADC needs port.pin not a symbol or pinX.Y 'SYMBOL toeRfeeler =D.5 'READADC needs port.pin not a symbol or pinX.Y HIGH whiskerLLED HIGH whiskerRLED HIGH feelerLLED HIGH feelerRLED GOSUB ReadWhiskersFeelers POKE RAM_whiskerLlit,W2temp1 POKE RAM_whiskerRlit,W2temp0 'sertxd("Wlit",#W2temp1,",",#W2temp0) POKE RAM_feelerLlit,W27temp1 POKE RAM_feelerRlit,W27temp0 'sertxd("Flit",#W2temp1,",",#W2temp0) LOW whiskerLLED LOW whiskerRLED LOW feelerLLED LOW feelerRLED GOSUB ReadWhiskersFeelers POKE RAM_whiskerL,W2temp1 POKE RAM_whiskerR,W2temp0 'sertxd("Wdark",#W2temp1,",",#W2temp0,cr) POKE RAM_feelerL,W27temp1 POKE RAM_feelerR,W27temp0 'sertxd("Fdark",#W2temp1,",",#W2temp0,cr) RETURN '---------------------------------------------------- ReadWhiskersFeelers: W2temp =0 W27temp =0 for cmnd =0 to 6 READADC A.5,sense0 : W2temp1 =W2temp1 MIN sense0 READADC D.0,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 =D.0 'READADC needs port.pin not a symbol sense0 =0 HIGH whiskerLLED HIGH whiskerRLED GOSUB ReadWhiskers POKE RAM_whiskerLlit,W2temp1 POKE RAM_whiskerRlit,W2temp0 sertxd(CR,"Wlit L",#W2temp1,",R",#W2temp0) LOW whiskerLLED LOW whiskerRLED GOSUB ReadWhiskers POKE RAM_whiskerL,W2temp1 POKE RAM_whiskerR,W2temp0 sertxd(CR,"Wdark L",#W2temp1,",R",#W2temp0,CR) RETURN '---------------------------------------------------- ReadWhiskers: W2temp =0 for cmnd =0 to 6 READADC A.5,sense0 : W2temp1 =W2temp1 MIN sense0 'whiskerL READADC D.0,sense0 : W2temp0 =W2temp0 MIN sense0 'whiskerR next cmnd RETURN '---------------------------------------------------- Qfeelers: 'SYMBOL Lfeeler =D.6 'READADC needs port.pin not a symbol 'SYMBOL Rfeeler =D.5 'READADC needs port.pin not a symbol HIGH feelerLLED HIGH feelerRLED GOSUB ReadFeelers POKE RAM_FeelerLlit,W2temp1 POKE RAM_FeelerRlit,W2temp0 sertxd(CR,"Flit L",#W2temp1,",R",#W2temp0) LOW feelerLLED LOW feelerRLED GOSUB ReadFeelers POKE RAM_FeelerL,W2temp1 POKE RAM_FeelerR,W2temp0 sertxd(CR,"Fdark L",#W2temp1,",R",#W2temp0,CR) 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_bg_Qtoes=1 'sertxd("Qtoes") sense0 =0 bit1 =bit1 OR senseLtoe bit0 =bit0 OR senseRtoe sertxd("%",#bit1,#bit0," ") RETURN '---------------------------------------------------- Qfell: sertxd("Qfell") 'Check if fallen 'GetUp: does GOSUB tsh, GOSUB tshh, GOSUB sshhh, while deciding if fallen over 'SYMBOL tipF =pinD.5 'SYMBOL tipB =pinD.6 'SYMBOL pin_tipF =D.5 'SYMBOL pin_tipB =D.6 'can't reliably read digital pins if used for analog 'INPUT pin_tipF doesn't make a difference 'INPUT pin_tipB doesn't make a difference 'sertxd(CR,"**F-",#pin_tipF,#pin_tipB,"-B**") 'mostly gives 29 and 30 FOR W2temp0=0 TO 1 READADC D.5,W27temp0 'read Rfoot feeler, ie if tip F should be 0 READADC D.6,W27temp1 'read Lfoot feeler, ie if tip B should be 0 IF W27temp0<5 OR W27temp1<5 THEN '5 just some number >0 Sspeed =Sspeed MIN 2 -1 GOSUB sshhh PAUSE 100 ELSE RETURN ENDIF NEXT IF f_b_react=0 THEN RETURN ENDIF '---------------------- GetUp: 'ua 'sertxd("GetUp") READADC D.5,W27temp0 'read Rfoot feeler, ie if tip F should be 0 READADC D.6,W27temp1 'read Lfoot feeler, ie if tip B should be 0 SERTXD("F",#W27temp0,"B",#W27temp1) IF W27temp1<10 THEN GOTO getUpB IF W27temp0<10 THEN GOTO getUpF RETURN '==================================================== ' REACTIONS '==================================================== Robject: SERTXD("ru") IF f_b_react=0 THEN SERTXD("q") RETURN ENDIF AutoObject: '2023-01-03 PEEK RAM_USawareAt,sense0 IF usdistobstacles 0=>free '2023-01-03 PAUSE 100 IF sense0=%11 THEN :ReactAt =OLR :ENDIF IF sense0=%10 THEN :ReactAt =OL_ :ENDIF IF sense0=%01 THEN :ReactAt =O_R :ENDIF RETURN '---------------------------------------------------- Rhole: 'sens0 set in Qground: SERTXD("rg") IF f_b_react=0 THEN SERTXD("q") RETURN ENDIF SERTXD("%",#bit1,#bit0) ' %LR 0=>holes 1=>on ground ' %00 should never happen if f_bg_Qtoes=1 Autohole: GOSUB bip ' sertxd(" hole",CR) ' %LR 0=>holes 1=>on ground '2023-01-03 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 '==================================================== ' ACTS '==================================================== WaveL: 'a5, aW 'sertxd("WaveR") 'Sspeed =Sspeed 'speed 9 is ok ' standOnLR=%01 GOSUB PR_SR RRto =RRin +20 LRto =LRo90 GOSUB Move RPto =RPC -TSO LPto =LPC +40 GOSUB Move LPto =LPC -40 GOSUB Move GOSUB Move LPto =LPC +40 GOSUB Move LPto =LPC +TSO GOSUB PR_SLR ' standOnLR=%11 'PEEK RAM_Sspeed,Sspeed RETURN '==================================================== ' ACTIONS '========================== InitLegs: sertxd("initlegs") Sframe =30 'do it slowly f_LRpulse =0 'don't pulse LRoll f_RRpulse =0 'don't pulse RRoll RPat =RPC -TSO 'Servo Right Pace At right foot fwd LPat =LPC +TSO 'Servo Left Pace At FOR cmnd =0 TO 20 '50=some number 'Hips@=mid => feet flat on ground GOSUB TellServos NEXT f_LRpulse =1 'don't pulse LRoll f_RRpulse =1 'don't pulse RRoll LRat =LRoeasy RRat =RRoeasy FOR cmnd =0 TO 20 '50=some number 'Hips@=mid => feet flat on ground GOSUB TellServos NEXT Sframe =DSframe 'LOOPs at ~40Hz/25ms set back to default RETURN '---------------------------------------------------- getUpF: 'uf 'get up from on front SERTXD("LieFU ") IF Sspeed<9 THEN :Sspeed = Sspeed MAX 3 :ENDIF 'doesn't get up if faster <9 GOSUB poseForwards GOSUB poseSit pause 500 GOSUB Standeasy PEEK RAM_Sspeed,Sspeed RETURN ' GOTO getUpFB '----- getUpB: 'ub 'get up from on back SERTXD("LieBU ") GOSUB poseback GOSUB poseSit GOSUB posebackfull GOSUB Standeasy RETURN '---------------------------------------------------- StampRL: 'a4 'sertxd("StampRL") GOSUB StampR GOSUB StampL RETURN '----------------------- StampR: ''aR 'sertxd("StampR") hframes =12 'about 1/2 sec GOSUB PR_SL Sspeed =Sspeed +3 MAX 8 hframes =12 'about 1/2 sec GOSUB PR_SLR PEEK RAM_Sspeed,Sspeed RETURN '----------------------- StampL: ''aL 'sertxd("StampL") hframes =12 'about 1/2 sec GOSUB PR_SR Sspeed =Sspeed +3 MAX 8 hframes =12 'about 1/2 sec GOSUB PR_SLR PEEK RAM_Sspeed,Sspeed RETURN '----------------------- kickL: 'aL GOSUB PR_SR temp0 =kickcmnd -"0" *11 LRto =LRat +temp0 GOTO kicked '----------------------- kickR: 'aR GOSUB PR_SL temp0 =kickcmnd -"0" *11 RRto =RRat +temp0 GOTO kicked '----------------------- KickOut: 'a6, ak, K sertxd("k",kickcmnd) temp0 =kickcmnd -"0" *11 LRto =LRstand +temp0 RRto =RRstand +temp0 standOnLR=%00 kicked: GOSUB Move GOSUB PR_SLR RETURN '----------------------- StandEasy: 'S LPto =LPC +TSO RPto =RPC -TSO standUp: 'a8, au GOTO poseEasy '==================================================== MOVEMENT: '==================================================== Hlt: 'stand with right foot forward IF f_Crawl=1 THEN crawlHalt IF f_b_F=1 THEN IF LPat>LPC THEN GOSUB PR_SR ELSE GOSUB PR_SL ENDIF GOSUB PP_HHO GOSUB PR_SLR ELSE IF LPat>LPC THEN GOSUB PR_SL ELSE GOSUB PR_SR ENDIF GOSUB PP_HHO GOSUB PR_SLR ENDIF RETURN '----------------------- Fd: 'SERTXD("F") IF f_Crawl=1 THEN crawlForwards IF LPat>LPC THEN GOSUB PR_SR GOSUB PP_WFB GOSUB PR_SLR ELSE GOSUB PR_SL GOSUB PP_WBF GOSUB PR_SLR ENDIF f_b_F =1 RETURN '----------------------- Bk: 'SERTXD("B") IF f_Crawl=1 THEN crawlBack IF LPat>LPC THEN GOSUB PR_SL GOSUB PP_WFB GOSUB PR_SLR ELSE GOSUB PR_SR GOSUB PP_WBF GOSUB PR_SLR 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 '----------------------- Tother: 'TurnOther PEEK RAM_P1_f8_flags,sense0 'see which turn was last IF bit0=0 THEN Lt GOTO Rt '----------------------- Tt: 'TurnTo IF LPat<=LPC THEN Lt GOTO Rt '----------------------- Ta: 'TurnAway: IF LPat>=LPC THEN Lt 'else drop through to Rt '----------------------- Rt: 'SERTXD("R") IF f_Crawl=1 THEN crawlRight IF f_b_F=1 THEN 'going forwards WORKS 'sertxd("dir forward") IF RPatLPC THEN 'Right foot is back 'sertxd(cr,"R back",cr) GOSUB PR_SL LPto =LPC -FT RPto =RPC ELSE 'Right foot is forward 'sertxd("dir back") GOSUB PR_SR LPto =LPC +FT RPto =RPC ENDIF ENDIF GOSUB MOVE GOSUB PR_SLR 'remember turn PEEK RAM_P1_f8_flags,sense0 :bit0 =0 :POKE RAM_P1_f8_flags,sense0 RETURN '----------------------- Lt: 'SERTXD("L") IF f_Crawl=1 THEN crawlLeft IF f_b_F=1 THEN 'going forwards WORKS 'sertxd("dir forward") IF LPatRPC THEN 'Right foot is back 'sertxd(cr,"R back",cr) GOSUB PR_SR RPto =RPC -FT LPto =LPC ELSE 'Right foot is forward 'sertxd("dir back") GOSUB PR_SL RPto =RPC +FT LPto =LPC ENDIF ENDIF GOSUB MOVE GOSUB PR_SLR 'remember turn PEEK RAM_P1_f8_flags,sense0 :bit0 =1 :POKE RAM_P1_f8_flags,sense0 RETURN '----------------------- crawlHalt: FL =FL /2 MAX 20 'FLcmnd(5)=>50, /2 =>25 otherwise fibula hits tibia pivot LRto =LRlie +FL MAX LRomax RRto =RRlie +FL MAX RRomax LPto =LPC RPto =RPC GOTO endCrawl '---------------- crawlForwards: sertxd("cF") FL =FL /2 MAX 20 'FLcmnd(5)=>50, /2 =>25 otherwise fibula hits tibia pivot LRto =LRlie +FL MAX LRomax RRto =RRlie +FL MAX RRomax LPto =LPC +FP RPto =RPC +FP GOSUB MOVE LRto =LRlie -FL RRto =RRlie -FL GOSUB MOVE IF senseLtoe=0 OR senseRtoe=0 AND f_b_autosense=1 THEN Qhole LPto =LPC -FP RPto =RPC -FP GOTO endCrawl '---------------- crawlBack: sertxd("cB") FL =FL /2 MAX 20 'FLcmnd(5)=>50, /2 =>25 otherwise fibula hits tibia pivot LRto =LRlie +FL MAX LRomax RRto =RRlie +FL MAX RRomax LPto =LPC -FP RPto =RPC -FP GOSUB MOVE LRto =LRlie -FL RRto =RRlie -FL GOSUB MOVE LPto =LPC +FP RPto =RPC +FP GOTO endCrawl '---------------- crawlRight: sertxd("cR") FL =FL /2 MAX 20 'FLcmnd(5)=>50, /2 =>25 otherwise fibula hits tibia pivot LRto =LRlie +FL MAX LRomax RRto =RRlie +FL MAX RRomax LPto =LPC +FT RPto =RPC -FT GOSUB MOVE LRto =LRlie -FL RRto =RRlie -FL GOSUB MOVE IF senseLtoe=0 AND f_b_autosense=1 THEN Qhole LPto =LPC -FT RPto =RPC +FT 'remember turn PEEK RAM_P1_f8_flags,sense0 :bit0 =0 :POKE RAM_P1_f8_flags,sense0 GOTO endCrawl '---------------- crawlLeft: sertxd("cL") FL =FL /2 MAX 20 'FLcmnd(5)=>50, /2 =>25 otherwise fibula hits tibia pivot LRto =LRlie +FL MAX LRomax RRto =RRlie +FL MAX RRomax LPto =LPC -FT RPto =RPC +FT GOSUB MOVE LRto =LRlie -FL RRto =RRlie -FL GOSUB MOVE IF senseRtoe=0 AND f_b_autosense=1 THEN Qhole LPto =LPC +FT RPto =RPC -FT 'remember turn PEEK RAM_P1_f8_flags,sense0 :bit0 =1 :POKE RAM_P1_f8_flags,sense0 ' GOTO endCrawl '---------------- endCrawl: GOSUB MOVE GOSUB poseSit PEEK RAM_leglLift,FL RETURN '---------------- Qhole: sertxd("Qh") GOSUB Qground 'prints % sets sense0 from toes IF sense0<>3 THEN Rhole RETURN '----------------------- stepL: LRto =LRC +FL RRto =RRC GOSUB MOVE LRto =LRC RRto =RRC +FL GOSUB MOVE RRto =RRC GOSUB MOVE RETURN stepR: LRto =LRC RRto =RRC +FL GOSUB MOVE LRto =LRC +FL RRto =RRC GOSUB MOVE LRto =LRC GOSUB MOVE RETURN '==================================================== ' POSE '==================================================== poseForwards: 'pf LPto =LPC -FP '20 RPto =RPC -FP '20 GOTO MOVE poseBack: 'pb LPto =LPC +FP '20 RPto =RPC +FP '20 GOTO MOVE poseMid: 'pm LPto =LPC RPto =RPC GOTO MOVE poseForwardsFull: 'pF LPto =LPb90 RPto =RPb90 GOTO MOVE poseBackFull: 'pB LPto =225 'LPmax +10 RPto =172 'RKmin +10 GOTO MOVE poseUp: 'pu LRto =LRC +FL RRto =RRC +FL GOTO MOVE poseEasy: 'pe LRto =LRoeasy RRto =RRoeasy standOnLR=%11 GOTO MOVE poseSit: 'ps 'a7, as LRto =LRlie RRto =RRlie standOnLR=%00 GOTO MOVE poseLskid: 'pl LPto =LPC +FT RPto =RPC -FT GOTO _skidit poseRskid: 'pr LPto =LPC -FT RPto =RPC +FT _skidit: temp0 =RRat -RRC +LRat -LRC IF temp0 < Fclear THEN LRto =LRoeasy RRto =RRoeasy ENDIF GOTO MOVE '----------------------- PR_SLR: 'Stand on Left + Right 'SERTXD("SLR ") IF LRat>=LRC AND RRC<=RRat THEN PR_standup IF RRat<=RRC THEN 'standing on Right LRto =LRC +RRC -RRat GOSUB MOVE ELSE 'standing on Left RRto =RRC +LRC -LRat GOSUB MOVE ENDIF PR_standup: ' Sspeed =Sspeed /2 MIN 1 'stops the move being jerky, MIN 1 otherwise hangs ' f_bmodified =1 RRto =RRC LRto =LRC standOnLR =%11 'any toe off is tested at Qfell GOTO MOVE PR_SR: 'Stand on Right 'SERTXD("SR ") PEEK RAM_FootIn,temp0 RRto =RRC -temp0 MIN RRin LRto =LRC +FL MAX 255 standOnLR =%01 GOTO MOVE PR_SL: 'Stand on Left 'SERTXD("SL ") PEEK RAM_FootIn,temp0 LRto =LRC -temp0 MIN LRin RRto =RRC +FL MAX 255 standOnLR =%10 GOTO MOVE '----------------------- 'PK_TLMMO: 'Turn Left Toes Mid Mid Offset '* LKto =LKat +FT MAX LKC -TSO '* RKto =RKat -FT MIN RKC +TSO ' GOTO MOVE 'PK_TRMMO: 'Turn Right Toes Mid Mid Offset '* LKto =LKat -FT MIN LKC +TSO '* RKto =RKat +FT MAX RKC -TSO ' GOTO MOVE '----------------------- PP_WFB: 'Walk FB LPto =LPC MIN FP -FP RPto =RPC +FP MAX 255 GOTO MOVE PP_WBF: 'Walk BF LPto =LPC +FP MAX 255 RPto =RPC MIN FP -FP GOTO MOVE PP_HHO: 'Toes Halt Offset PP_MSO: 'Toe Mid Stand - toes offset LPto =LPC +TSO RPto =RPC -TSO GOTO MOVE '----------------------- PP_FF: LPto =LPC +85 MAX LPfwd RPto =RPC +85 MAX RPfwd GOTO MOVE 'PK_MS: 'Pose MS 'Toe Mid Stand - toes offset ' LKto =LKC ' RKto =RKC ' GOTO MOVE PP_BB: LPto =LPC -85 MIN RPback RPto =RPC -85 MIN RPback 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 '==================================================== MOVE: '========================== ' SERTXD("move" ) f_RRdo =1 'doing all sevos f_RPdo =1 'doing all sevos f_LRdo =1 'doing all sevos f_LPdo =1 'doing all sevos LPto =LPto MIN LPback MAX LPfwd LRto =LRto MIN LRin MAX LRomax RPto =RPto MIN RPback MAX RPfwd RRto =RRto MIN RRin MAX RRomax 'topple =0 '---------------------------------------------------- MoveCalcs: 'sertxd("movecalcs") 'sertxd(#f_LAdo, #f_LHdo, #f_RHdo, #f_RAdo,cr) 'sertxd("AtTo",#LHat," ",#LHto," ",#RHat," ",#RHto," ",#Sspeed) '------------- _RR: 'Right Roll SELECT RRat CASE =RRto f_RRdo =0 CASE >RRto RRat =RRat MIN Sspeed -Sspeed MIN RRto CASE RPto RPat =RPat MIN Sspeed -Sspeed MIN RPto CASE LRto LRat =LRat MIN Sspeed -Sspeed MIN LRto CASE LPto LPat =LPat MIN Sspeed -Sspeed MIN LPto CASE stance toe off ground IF f_b_Qground=1 THEN IF standOnLR=%10 THEN IF senseLtoe=0 THEN : topple =topple +1 :ENDIF ENDIF IF standOnLR=%01 THEN IF senseRtoe=0 THEN : topple =topple +1 :ENDIF ENDIF IF topple>50 THEN AbortMove 'on faster speeds bounce never gets to 5 ENDIF 'f_b_Qground #endrem W2pulselo = f_LPdo +f_LRdo +f_RPdo +f_RRdo '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 THEN Run0 'quit Intrinsic Act now done AbortMove RETURN #rem AbortMove: ' toe 0=off ground -> falling? so abortMove and do SLR sertxd("toppled") Sspeed =9 :f_Bmodified =1 : f_MoveAborted =1 : f_LKpulse =0: f_RKpulse =0 GOTO PH_SLR #endrem '====================================================