'Loki_8-1.bas 'Loki_7-1.bas 'Ullr_6-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 = 3722 bytes out of 4096 #rem ============================================ PROG1 ----- Init: Qfell: SELECT cmnd -------- ' BEHAVIOUR NewBehave: -------- ' MIND NewMapWhiskersFeelers: MapWhiskersFeelers: NewMapWhiskers: MapWhiskers: NewMapFeelers: MapFeelers: -------- ' SENSES {setUSawares: {setUSawares_m: Qping: Qwhiskerfeelers: Qwhiskers: Qfeelers: Qground: Qhole: -------- ' REACTIONS Robject: Robstacle: Rhole: -------- ' ACTS WaveL: rollroll: -------- ' ACTIONS InitLegs: {getUpF: 'get up from on front {getUpB: 'get up from on back {getUp: 'get up StampRL: 'a4 StampR: ''aR not called StampL: ''aL not called kickL: 'aL kickR: 'aR KickOut: 'a6, ak, K StandEasy: 'S: -------- ' MOVEMENT: Hlt: 'stand with right foot forward Fd: Bk: Tm: 'Turn Memory To: 'Turn Other Tt 'Turn To Tf: 'Turn Foot Ta: 'Turn Away ' Tr: 'Turn R on spot Tl: 'Turn L on spot Rt: Lt: stepL: stepR: -------- ' POSE poseForwards: poseBack: poseUp: PP_WBF: 'Walk BF PP_WFB: 'Walk FB PP_HHO: 'Pose HH Toes Halt Offset PP_MMO: 'Pose MM Toes Mid Mid Offset PP_FF: 'Pose FF PP_BB: 'Pose BB PR_SLR: 'Stand on Left + Right PR_SR: 'Stand on Right PR_SL: 'Stand on Left -------- ' HMI bip: sshhh: 'because that is what it sounds like beep255 whee: 'make rising sound whoa: 'make falling sound: 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 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 '================================================== '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 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 '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 '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 ' only used in QIR: and set to 0 here for F B R L '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 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 #rem Rtoe up to miss L LA131 LH97 RH152 RA90 logical Ltoes forward 90 LA131 LH97 RH152 RA90 logical both R L toes forward - lean forward a bit so don't overbalance back LA131 LH97 RH152 RA90 logical max lean forward LA62 LH196 RH241 RA38 logical #endrem '-------------------------------------------------------------- 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 '================================================== ' 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 't a p u $ ? CASE "P" GOSUB Pos '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 Tr 'Turn R on the spot CASE "l" GOSUB Tl 'Turn L on the spot 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 "4","v" GOSUB beep100 '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","r" GOSUB rollroll 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 "s" GOSUB poseSit :f8_doServos =0 'no pulsing in prog0 CASE "f" GOSUB poseForwards 'at FP CASE "m" GOSUB poseMidO CASE "b" GOSUB poseBack 'at FP 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 'CaseSense CASE "$" 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 "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 "?" 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 :lastcmnd =0 'allow repeat keys in QIR: CASE "B" GOSUB Bk :lastcmnd =0 'allow repeat keys in QIR: CASE "R" GOSUB Rt :lastcmnd =0 'allow repeat keys in QIR: CASE "L" GOSUB Lt :lastcmnd =0 'allow repeat keys in QIR: CASE "K" GOSUB KickOut CASE "H" GOSUB Hlt ' CASE "V" GOSUB bip 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_RHC,RHC :RHC =RHC +TOO 'RHC set in @ READ EEPROM_LHC,LHC :LHC =LHC +TOO 'LHC set in @ READ EEPROM_RAC,RAC 'RAC set in @ READ EEPROM_LAC,LAC 'LAC set in @ Sspeed =Scmnd -"0" MIN 1 'otherwise MOVE: hangs POKE RAM_Sspeed,Sspeed FL =FLcmnd -"0" *6 '0=0/19 1=6/19 2=12/19 3=18/19 4=24 5=30 6=36 7=42 8=48 9=54 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" *3' +2 '0=2 1=5 2=8 3=11 4=14 5=17 6=20 7=23 8=26 9=30 <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 SpeedDown: Sspeed =Sspeed /2 MIN 1 Scmnd ="0" +Sspeed f_Bmodified =1 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) '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 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) '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_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 - does 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 'ELSE GetUp '---------------------- 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 W27temp0<3 THEN GOTO getUpF IF W27temp1<3 THEN GOTO getUpB RETURN '==================================================== ' REACTIONS '==================================================== Robject: SERTXD("ru") IF f_b_Uon=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 Autohole: FOR temp55 =1 to 5 'maybe a bounce so retest IF sense0=3 THEN RETURN ENDIF GOSUB bip GOSUB PA_SLR GOSUB Qground '2023-01-03 moved to last in NEXT 'sertxd(#temp55," ") NEXT 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") ' standOnLR=%01 GOSUB PA_SL RAto =RAC -Fclear RHto =RHC +FP GOSUB Move RHto =RHC -FP GOSUB Move RHto =RHC +FP GOSUB Move RHto =RHC -FP GOSUB Move RHto =RHC GOSUB Move GOSUB PA_SLR standOnLR=%11 RETURN rollroll: RAto =RAC -FL LAto =LAC GOSUB Move RAto =RAC LAto =LAC -FL GOSUB Move RAto =RAC -FL LAto =LAC GOSUB Move RAto =RAC GOSUB Move RETURN '==================================================== ' ACTIONS '========================== InitLegs: sertxd("initlegs") Sframe =30 'do it slowly f_LApulse =1 'pulse LA f_RApulse =1 'pulse RA LAat =LAstand +Fclear RAat =RAstand -Fclear FOR cmnd =0 TO 10 '50=some number 'Hips free, stand on left GOSUB TellServos NEXT f_LHpulse =1 'pulse LH f_RHpulse =1 'pulse RH RHat =RHC +TSO 'Servo Right Pace At right foot fwd LHat =LHC -TSO 'Servo Left Pace At FOR cmnd =0 TO 10 '20=some number 'Hips@=midO GOSUB TellServos NEXT LAat =LAstand RAat =RAstand FOR cmnd =0 TO 20 '20=some number 'Hips@=midO, 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 Sspeed =Sspeed MAX 4 :f_Bmodified =1 'getup from face 'to full bow - max speed 4 'P#196#131#245#100 P#196#055#245#036 P#196#125#245#105 LHto =196 :LAto =131 :RHto =245 :RAto =100 :GOSUB Move hframes =20 LAto = 55 :RAto = 36 :GOSUB Move LAto =125 :RAto =105 :GOSUB Move 'set legs to halt - works from full bow 'P#196#125#245#080 P#106#125#152#080 P#100#131#157#112 RAto = 80 :GOSUB Move LHto =106 :LAto =125 :RHto =152 :GOSUB Move getUpDone: LHto =100 :LAto =131 :RHto =157 :RAto =112 :GOSUB Move RETURN '----- getUpB: 'ub 'get up from on back Sspeed =Sspeed MIN 2 :f_Bmodified =1 'P#026#140#060#112 P#026#055#060#030 LHto = 26 :LAto =140 :RHto = 60 :RAto =112 :GOSUB Move hframes =50 LAto = 55 :RAto = 30 :GOSUB Move 'P#026#131#060#112 P#100#131#157#090 P#100#131#157#112 LAto =131 :RAto =112 :GOSUB Move LHto =100 :RHto =157: RAto = 90 :GOSUB Move GOTO getUpDone ' RETURN '---------------------------------------------------- StampRL: 'a4 'sertxd("StampRL") GOSUB StampR GOSUB StampL RETURN '----------------------- StampR: ''aR 'sertxd("StampR") hframes =12 'about 1/2 sec GOSUB PA_SL hframes =12 'about 1/2 sec GOSUB PA_SLR RETURN '----------------------- StampL: ''aL 'sertxd("StampL") hframes =12 'about 1/2 sec GOSUB PA_SR hframes =12 'about 1/2 sec GOSUB PA_SLR RETURN '----------------------- kickL: 'aL GOSUB PA_SR temp0 =kickcmnd -"0" *11 LAto =LAat +temp0 GOTO kicked '----------------------- kickR: 'aR GOSUB PA_SL temp0 =kickcmnd -"0" *11 RAto =RAat +temp0 GOTO kicked '----------------------- KickOut: 'a6, ak, K sertxd("k",kickcmnd) temp0 =kickcmnd -"0" *6 LAto =LAC +temp0 RAto =RAC +temp0 standOnLR=%00 kicked: GOSUB Move GOSUB PA_SLR RETURN '==================================================== MOVEMENT: '==================================================== Hlt: 'stand with right foot forward IF f_b_F=1 THEN IF LHat=LHC AND RHatRHC THEN Rt GOTO Lt ENDIF '----------------------- Ta: 'TurnAway: IF f_b_F=1 THEN IF LHat>=LHC AND RHat<=RHC THEN Rt GOTO Lt ELSE IF LHat<=LHC AND RHat>=RHC THEN Lt GOTO Rt ENDIF '----------------------- Tr: 'Turn Right on the spot IF RHat=RHC THEN 'Right foot is forward 'sertxd(cr,"R forward",cr) GOSUB PA_SR RHto =RHC -FT LHto =LHC ELSE 'Right foot is back 'sertxd("dir back") GOSUB PA_SL RHto =RHC +FT LHto =LHC ENDIF ELSE 'going back WORKS 'sertxd("dir back") IF LHat=LHC THEN 'Right foot is forward 'sertxd(cr,"Lforward",cr) GOSUB PA_SL LHto =LHC -FT RHto =RHC ELSE 'Right foot is back 'sertxd(cr,"Rforward",cr) GOSUB PA_SR LHto =LHC +FT RHto =RHC ENDIF ELSE 'going back WORKS 'sertxd("dir back") IF RHat so FL=<6-8>*6 '6=FL scale RAto =RAC -FL hframes =Sspeed 'let move complete GOSUB MOVE RAto =RAC hframes =Sspeed 'let move complete GOSUB MOVE RETURN stepR: FL =FLcmnd -"0" +6 MAX 8 *6 :f_Bmodified=1 'FLcmnd<0-9> so FL=<6-8>*6 '6=FL scale LAto =LAC -FL hframes =Sspeed 'let move complete GOSUB MOVE LAto =LAC hframes =Sspeed 'let move complete GOSUB MOVE RETURN '==================================================== ' POSITION '==================================================== Pos: 'Position 0 in, 5 mid, 9 out PEEK RAM_LHpose,temp0 :IF temp0>="0" AND temp0<="9" THEN :LHto =temp0 -"0" *16 -80 +LHC ENDIF PEEK RAM_LApose,temp0 :IF temp0>="0" AND temp0<="9" THEN :LAto =temp0 -"0" *16 -80 +LAC ENDIF PEEK RAM_RApose,temp0 :IF temp0>="0" AND temp0<="9" THEN :RAto =temp0 -"0" *16 -80 +RAC ENDIF PEEK RAM_RHpose,temp0 :IF temp0>="0" AND temp0<="9" THEN :RHto =temp0 -"0" *16 -80 +RHC ENDIF GOTO MOVE '==================================================== ' POSE '==================================================== poseForwards: 'pf LHto =LHC -FP '20 RHto =RHC -FP '20 GOTO MOVE poseBack: 'pb LHto =LHC +FP '20 RHto =RHC +FP '20 GOTO MOVE poseUp: 'pu LAto =LAC +FL RAto =RAC +FL GOTO MOVE '----------------------- PH_WFB: 'Walk FB RHto =RHC MIN FP -FP LHto =LHC +FP MAX 255 GOTO MOVE PH_WBF: 'Walk BF RHto =RHC +FP MAX 255 LHto =LHC MIN FP -FP GOTO MOVE '----------------------- poseMidO: 'pm PH_HHO: 'Toes Halt Offset PH_MMO: 'Toe Mid Mid - toes offset LHto =LHC -TSO RHto =RHC +TSO GOTO MOVE PH_FF: LHto =LHC +85 MAX LHout RHto =RHC +85 MAX RHout GOTO MOVE PH_BB: LHto =LHC -85 MIN RHin RHto =RHC -85 MIN RHin GOTO MOVE '----------------------- poseSit: PA_SLR: 'Stand on Left + Right 'SERTXD("SLR ") RAto =RAC +TOD LAto =LAC +TOD standOnLR =%11 'any toe off is tested at Qfell GOTO MOVE PA_SR: 'Stand on Right 'SERTXD("SR ") temp0 =FL MIN Fclear LAto =LAC -temp0 MIN RAin RAto =RAC +FL MAX RAout standOnLR =%01 GOTO MOVE PA_SL: 'Stand on Left 'SERTXD("SL ") temp0 =FL MIN Fclear RAto =RAC -temp0 MIN LAin LAto =LAC +FL MAX LAout standOnLR =%10 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_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 'topple =0 '---------------------------------------------------- MoveCalcs: 'sertxd("movecalcs") 'sertxd(#f_LAdo, #f_LHdo, #f_RHdo, #f_RAdo,cr) 'sertxd("AtTo",#LHat," ",#LHto," ",#RHat," ",#RHto," ",#Sspeed) '------------- _RA: 'Right 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 stance toe off ground IF f_b_autosense=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_LHdo +f_LAdo +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 RETURN #rem AbortMove: ' toe 0=off ground -> falling? so abortMove and do SLR sertxd("toppled") Sspeed =9 :f_Bmodified =1 : f_MoveAborted =1 hframes =20 LHto =LHmid +20 RHto =RHmid +20 f_LKpulse =0: f_RKpulse =0 standOnLR=%00 GOSUB Move GOTO PH_SLR #endrem '====================================================