'Thor_8-3.bas 40x2 Firmware v3 'Thor_7-3.bas 40x2 Firmware v3 #picaxe 40x2 #slot 3 #no_data '#no_table DISCONNECT SETFREQ m16 '#3 Memory used = 3922 bytes out of 4096 EEPROM,Execute '================================================== #rem IF cmnd ="I" THEN GOSUB initEEPROM IF cmnd =255 THEN GOSUB initEEPROM IF cmnd ="E" THEN START '================================================== #endrem '================================================== Pins: '================================================== SYMBOL servoLA =A.1 SYMBOL servoLH =A.0 SYMBOL servoRA =D.3 SYMBOL servoRH =D.2 SYMBOL whiskerLLED =A.6 SYMBOL whiskerRLED =A.7 SYMBOL whiskerL =A.5 'READADC needs port.pin not a symbol SYMBOL whiskerR =D.0 'READADC needs port.pin not a symbol SYMBOL EyeLLED =C.1 SYMBOL EyeRLED =C.2 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 feelerRLED =D.4 SYMBOL pingpin =C.0 SYMBOL voiceLED =C.5 SYMBOL voice =C.5 SYMBOL senseLtoe =pinA.2 SYMBOL senseRtoe =pinD.1 'SYMBOL senseLtoe =A.2 'SYMBOL senseRtoe =D.1 SYMBOL inIR =A.3 SYMBOL inIRpin =pinA.3 'SYMBOL O_LCD =A.4 'bus B.7 - B.0 '================================================== VarsCons: '================================================== SYMBOL Esc =27 'Escape key SYMBOL quote2 =34 'double quote mark 'built in variables 'bptr = RAM pointer 'ptr - scratchpad pointer 'hserptr - pointer to scratchpad location of last byte received by hardware UART 'X2 parts - bit0-31 b0-55 w0-27 +56 - 255 Peek/Poke 'W0 'SYMBOL sense0 =b0 SYMBOL temp0 =b0 SYMBOL f8_behaves =b1 'used in programs 0123 w=write r=read !=wr SYMBOL f_b_react =bit8 'wrr- 1=reacts on SYMBOL f_b_autosense =bit9 'w!r- 1= auto test sensors, "b$a" 'SYMBOL f_b_autoUp =bit10 'wrr- tested in Qfell, default=1 "bu" Baldur 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 Baldur 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_Rfwd =bit23 '-!-- SYMBOL f8_doServos =b3 'ww-- SYMBOL f_RAdo =bit24 '-!-! do servo SYMBOL f_RHdo =bit25 '-!-! do servo SYMBOL f_LAdo =bit26 '-!-! do servo SYMBOL f_LHdo =bit27 '-!-! do servo SYMBOL f_RApulse =bit28 'r!-r pulse servos SYMBOL f_RHpulse =bit29 'r!-r pulse servos SYMBOL f_LApulse =bit30 'r!-r pulse servos SYMBOL f_LHpulse =bit31 'r!-r pulse servos 'W2 SYMBOL W2pulse =w2 SYMBOL W2pulselo =b4 SYMBOL W2pulsehi =b5 SYMBOL W2temp =w2 'Interpage jumps for hserptr SYMBOL W2templo =b4 SYMBOL W2temphi =b5 'SYMBOL W2temp0 =b4 'SYMBOL W2temp1 =b5 'W3 SYMBOL cmnd =b6 SYMBOL RootCmnd =b7 'SYMBOL servonum =b7 'SETUP 'W4 SYMBOL RHC =b8 'Right Hip Centre for current posture SYMBOL LHC =b9 'Left Hip Centre for current posture 'W5 SYMBOL RAC =b10 'Right Ankle Centre for current posture SYMBOL LAC =b11 'Left Ankle Centre for current posture 'W6 SYMBOL RHat =b12 'Servo Right Hip At SYMBOL RHto =b13 'Servo Right Hip To 'W7 SYMBOL LHat =b14 'Servo Left Hip At SYMBOL LHto =b15 'Servo Left Hip To 'W8 SYMBOL RAat =b16 'Servo Right Ankle At SYMBOL RAto =b17 'Servo Right Ankle To 'W9 SYMBOL LAat =b18 'Servo Left Ankle At SYMBOL LAto =b19 'Servo Left Ankle To 'W10 SYMBOL Sspeed =b20 '1-9 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 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 FUScmnd =b36 'Foot Up In = 9 -FUI *FL /9 Baldur 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 SYMBOL usdist =b41 'prog2 'W21 'SYMBOL Bo =b42 'lean back offset for backwards Baldur SYMBOL TOF =b43 'Toes Offset Front 1-9 '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 SYMBOL RHcmnd =b50 'controls Hip Centres for RideHeight 0-9 in 2-NewBehave: SYMBOL RHcmndold =b51 'last Ride height cmnd 'Thor 'W26 'SYMBOL stackcmnd =b53 'only set in #2 LoadBehaviour, only read here Qserial: 'W27 'SYMBOL W27temp =w27 'prog1 'SYMBOL W27temp0 =b54 'prog1 'SYMBOL W27temp1 =b55 'prog1 'SYMBOL temp54 =b54 ' 'SYMBOL temp55 =b55 'prog1 '--------- 'Variables - Storage - peek and poke '------------------- '200 56 to 255 ($38 to $FF) SYMBOL RAM_temp0 =56 'first non variable RAM location 'SYMBOL RAM_P1_f8_flags =57 'Prog1 flags 'turned Left bit 0 '1=last turned Left, 0=Right ''SYMBOL RAM_holedetectR =58 ' ''SYMBOL RAM_holedetectL =59 ' SYMBOL RAM_obstaclesR =60 'map SYMBOL RAM_obstaclesL =61 'map SYMBOL RAM_lightR =62 'whisker ambient light level Right SYMBOL RAM_lightL =63 'whisker ambient light level Left SYMBOL RAM_UStoonear =64 SYMBOL RAM_USawareAt =65 SYMBOL RAM_USunits_cm_ =66 SYMBOL RAM_USlast =67 SYMBOL RAM_WhiskerR =68 SYMBOL RAM_WhiskerL =69 SYMBOL RAM_WhiskerRlit =70 SYMBOL RAM_WhiskerLlit =71 SYMBOL RAM_FeelerR =72 SYMBOL RAM_FeelerL =73 SYMBOL RAM_FeelerRlit =74 SYMBOL RAM_FeelerLlit =75 SYMBOL RAM_Wthresh =76 SYMBOL RAM_Fthresh =77 'SYMBOL RAM_Sspeed =78 'SYMBOL RAM_randvarLo =68 'SYMBOL RAM_randvarHi =69 'SYMBOL RAM_randseedlo =70 'SYMBOL RAM_randseedhi =71 SYMBOL 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_TOF =251 'Toes Offset Front 1-9 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_RHcmnd =246 'RideHeight 1-9, (Baldur FUScmnd) '245 Bo 'Back lean back Baldur 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 PotSetRanges - values are in 10us units 'GWS S03 [39.5 x 20.0 x 35.6 42g] 0.23sec 3.4@4.8v 4@6v '================================================== SYMBOL SRHmin =126 '0 Right Hip Max foot up, full body down SYMBOL SRHmid =168 '42 Right Hip Stand feet flat SYMBOL SRHmax =210 '86 Right Hip Max foot down leg stop SYMBOL SRAmax =231 'Right Ankle Max out servo stop SYMBOL SRAF =224 'Toes Forward 'SYMBOL SRAFWmax =175 '+33 back toe hits other foot SYMBOL SRAFWmid =162 '+20 SYMBOL SRAFWmin =147 '+5 TOS=5 SYMBOL SRAmid =142 'Right Ankle Mid ahead - walk range +-20 TOS=5 SYMBOL SRABWmin =138 '-5 TOS=5 SYMBOL SRABWmid =112 '-20 'SYMBOL SRABWmax =101 '-41 front toe hits other foot SYMBOL SRAB =61 'Toes Back SYMBOL SRAmin =55 'Right Ankle Min in servo stop SYMBOL SLHmax =197 '0 Left Hip Max foot up, full body down 67 SYMBOL SLHmid =152 '45 Left Hip Stand feet flat 125 SYMBOL SLHmin =104 '93 Left Hip Max foot down leg stop 157 SYMBOL SLAmin =58 'Left Ankle Min out servo stop SYMBOL SLAF =67 'Toes Forward 'SYMBOL SLAFWmax =111 '-33 back toe hits other foot SYMBOL SLAFWmid =124 '-20 SYMBOL SLAFWmin =139 '-5 TOS=5 SYMBOL SLAmid =144 'Left Ankle Mid ahead - walk range +-20 TOS=5 SYMBOL SLABWmin =149 '+5 TOS=5 SYMBOL SLABWmid =164 '+20 'SYMBOL SLABWmax =178 '+33 front toe hits other foot SYMBOL SLAB =231 'Toes Back SYMBOL SLAmax =238 '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 '231 -55 =176 SYMBOL RAFWmid =SRAFWmid '162 SYMBOL RAFWmin =SRAFWmin SYMBOL RAmid =SRAmid '142 SYMBOL RABWmin =SRABWmin SYMBOL RABWmid =SRABWmid '112 SYMBOL RAmin =SRAmin '55 SYMBOL RHmin =SRHmin '126 -211 =85 'foot up SYMBOL RHmid =SRHmid '168 SYMBOL RHmax =SRHmax '213 'foot down 'note max min reversals for left SYMBOL LAmax =255 -SLAmin '255 -58 =197 197 -17 =180 SYMBOL LAFWmid =255 -SLAFWmid '255 -124 =131 SYMBOL LAFWmin =255 -SLAFWmin SYMBOL LAmid =255 -SLAmid '255 -144 =111 SYMBOL LABWmin =255 -SLABWmin SYMBOL LABWmid =255 -SLABWmid '255 -164 =91 SYMBOL LAmin =255 -SLAmax '255 -238 =17 SYMBOL LHmin =255 -SLHmax '58 - 152 =94 'foot up SYMBOL LHmid =255 -SLHmid '103 SYMBOL LHmax =255 -SLHmin '152 'foot down '--------------- 'for servo numbering in CaseAdjust: SYMBOL RA =0 SYMBOL RH =1 SYMBOL LA =2 SYMBOL LH =3 '-------------------------------------------------------------- '================================================================= ' INITIALISATION '========================== Init: hserptr =W2temp 'get back pointer IF cmnd<>"h" THEN :SERTXD("{3",RootCmnd,cmnd,",") :ENDIF '==================================================== ' COMMANDER '==================================================== IF cmnd ="h" THEN GOSUB PrintHelp Run0: W2pulse =hserptr 'Preserve pointer RUN 0 '============================================================= PrintHelp: SERTXD(CR,"HELP - {choice 5 is toes mid, 0 is foot full up = br0") SERTXD(CR," if comma then ignore eg P3,628") SERTXD(CR,"y{1-9 increments yawn each execution,") SERTXD(CR," if yawn=parameter then skip next cmnd") SERTXD(CR,"+ react ON, test WF sensors") SERTXD(CR,"- react ON, test G sensors") SERTXD(CR,"$ test sensors and react") SERTXD(CR," 0 reacts OFF, autosense OFF") SERTXD(CR," 1 reacts ON") SERTXD(CR," {2,a autosense ON") SERTXD(CR," + W & F") SERTXD(CR," U Ultrasonic") SERTXD(CR," W Whiskers") SERTXD(CR," F Feelers") SERTXD(CR," {G,- Ground sensors") SERTXD(CR,CR,"bS Save current behaves") SERTXD(CR," 0 load behaves") SERTXD(CR," I Initialise EEPROM") SERTXD(CR," L Lethargic") SERTXD(CR," N Normal") SERTXD(CR," V Vigorous") SERTXD(CR," v{l,n,h,x vitality low normal high extreme") SERTXD(CR," l{1-9 lift") SERTXD(CR," p{1-9 pace") SERTXD(CR," t{1-9 turn") SERTXD(CR," r{0-9 ride height") SERTXD(CR," s Squat") SERTXD(CR," c Crouch") SERTXD(CR," t Tall") SERTXD(CR," f<1-9 Toe front offset") SERTXD(CR," $a autosense ON, [9][+]=OFF") SERTXD(CR," $1 sensors reactions ON [9][-]=OFF") SERTXD(CR," $0 sensors autosense and reactions OFF [9][+][9][-]") SERTXD(CR," WS{0-9 Whisker threshold decade") SERTXD(CR," {0-9 Whisker threshold digit") SERTXD(CR," FS<1-9 Feeler threshold decade") SERTXD(CR," {0-9 Feeler threshold digit") SERTXD(CR," U{0,1 U/S reacts OFF,ON") SERTXD(CR," U+{1-9 Ultrasonic AwareAt units 50mm/2in") SERTXD(CR," U-{1-9 Ultrasonic TooNear units 50mm/2in") SERTXD(CR," @ Adjust joints - [/]next,[*]print,[-][+]adjust,[C]centre,[Q]") SERTXD(CR,CR,"?{h,^,?,{1-8,p,b,d,#") SERTXD(CR," print- Help,Subs,Acts,Act,Pose,BeHaves,dump-Acts,#Acts") SERTXD(CR,"?{U,W,F,G - print Sonar,Whiskers,Feelers,Ground") SERTXD(CR,"?{!.M print- Reacts,Mind",CR) RETURN '=============================================================