'>Frea8.bsx 'Mini Frea ' {$PBASIC 2.5} '{$stamp bs2sx,Frea8_1,Frea8_2} 'Summary '------- '@24Mar11 'Frea8 13Mar11 added IRwhiskers 'Frea7 09Apr09 'Frea6 06Apr09 Kicker ' has a bug in standonright and standonleft (and elsewhere?) ' in standonright FOR S_stepspeed>4 ' LHat =LHat -S_stepspeed MIN LHwu goes through zero AND LHat=255 ' Frea kicks out sideways but otherwise seems to walk ok. ' Interesting bug, Frea acts as if she is mad at something. 'Frea4 19Oct06 'Frea3 26-8-05 'Frea2 22/3/04 '================================================================ '>Frea8_0.bsx 'BRANCH avoidctrl,[Explore,_Bk,_Bk,_Turn] ' Explore: dovision: and avoid, else dornd:, DoStart: ' dornd: <100 - 100 SeekLight: or SeekDark: depending on lovelight ' <200 - 100 Do_FD ' <215 - 15 Do_TURN depends on f_RT, only set in TestToes: =>memory of bad ' <220 - 5 Do_RT ' <225 - 5 Do_LT ' <230 - 5 Do_BK ' <240 - 10 Krellish ' else - 15 R_Squak3 'this is my space but I am not threatening you '>Frea8_1.bsx 'BRANCH cmnd,[,_FD,_BK,_TURN,_RT,_LT,_STRETCH] '>Frea8_2.bsx 'BRANCH cmnd,[,_setlegs,_GetUp] '================================================================ 'Improvements '------------ 'Use spkr for RedLED as well then have one pin spare. 'If both tip sensors were across resistors and read by RCTIME then would have a spare pin 'If both tip sensors were on a PicAxe then that would leave the two pins for serial to the Picaxe ' PicAxe ' 08m2 - 2 tip ' 2 serial - BS2 ' 1 IR ' 1 spare 'Make four feelers, two each side, in parallel with toes, no more code needed 'In dornd: could read a flag and get the next cmnd from DATA in Prog3, keep the pointer in CRAM, ' prog3 runs prog1 which runs prog0 to check sensors, then if clear dornd runs prog3 to get next move. '================================================================ 'Behaviour '--------- 'redLED blinks at start of every Do loop 'greenLED on while vision avoiding 'redLED on indicates Light turn 'redLED + greenLED on indicates Dark turn 'yellow whisker LED on if IRwhisker detects object 'Do ' Case ' if fallen over (tip sensors activated) 2_GetUp ' If both feet off beep and flash redLED, sometimes(1/3) pause, sometimes(2/3) 1_STRETCH ' If one toe off backup and turn away ' IF IRwhisker detcts obstacle backup and turn away ' If obstacle detected using greenLED turn away ' Endcase ' Decide what to do ' seek/avoid light; ' behaviour alternates between seeking light and avoiding light ' if lovelight <=[lovelightthresh CON 24] seek dark, else seek light ' lovelight +1 every loop ' if light <=[toobright_ CON 20, ie very bright] then lovelight/2 ' or step forward; (1_FD) ' or turn away from last drop-off or whisker contact; (1_TURN) ' or step right; (1_RT) ' or step left; (1_LT) ' or step back; (1_BK) ' or make sound; ' or Krell LEDs ' MaxHappy set by Pot, ' Increment Happy +1 every loop, Happy sets speed ' Happy/2 if tilt or toes off floor '================================================================ 'Frea Spec '--------- 'body weight 125g 'total weight 225g '=>leg weight 50g 'hip centres 10cm 'hip torque required ' 5*125 =0.625Kgcm for body ' + 10*50 =0.5Kgcm for other leg ' ie 1.125Kgcm ' 'Ripmax SD200 servos = [Protech Super Micro] '+ve = anticlock [Protech B405 +ve = clockwise - not used now] '5v 0.45A at stall '@5cm torque force 330g =>1.65Kgcm ' pull out >600g =>3.0Kgcm '6v '@5cm torque force 350g =>1.75Kgcm ' pull out >700g =>3.5Kgcm '+--------------------------------------------------+ '| Ripmax SD200 [box information] | '| A high quality ballraced economy mini servo. | '| The SD200 features 4.2mm 1st and 2nd gears | '| for strength and reliability as well as 2.5Kg/cm | '| torque and 0.14/sec/60deg speed. | '| Torque: 2.5Kg/cm | '| Speed: 0.14 sec/60deg | '| 29.5 x 25 x 13.9mm | '| Weight: 16g | '+--------------------------------------------------+ ' 'Body Daewoo RubyRed(Pearl) on top of Wheel-silver 'Legs Humbrol Gunmetal on top of Black permanent marker, aluminium foil 'Feet Humbrol Steel on top of Black permanent marker 'all external wood sanding sealer'd '================================================================ 'Hardware problems '----------------- '25-8-05 blew toe inputs, pins work as outputs so swapped 9/10, 13/14 '================================================================ ' 'History '------- '28Jan13 broke LeftHip servo, changed for Protec but kept SD200 GBoxTop, LHM=260, had to reduce LHlie ' now LH doesn't jitter. '20Mar12 whisker pin6 changed to whisker_, remade toe springs (one had fallen off - gone) '24Mar11 changed some names from Test*: to Q*: '24Mar11 QThenWhatToDo: changed the PAUSE time from ~50% to ~1/3 '24Mar11 moved TestToes: to inline before IRWhiskers, was GOSUB'd from there before '23Mar11 added IRwhisker code '13Mar11 added IRwhiskers 'Frea8_1.bsx no change from Frea7_1.bs2 'Frea8_2.bsx no change from Frea7_2.bs2 '***Frea8*** '13Mar11 v_seenobject: moved, _vRemember: label added, added another BK to avoidctrl list '05Mar11 added Krellish to glow red and green LEDs as a resting phase. Renamed DoLoop as DoStart '05Mar11 moved speaker from pin2 to pin5, whisker LEDs will be logicaly better '12Apr09 added behaviour to avoid light ' if lovelight <=[lovelightthresh CON 24] seek dark, else seek light ' lovelight +1 every loop ' if light <=[toobright_ CON 20, ie very bright] then lovelight/2 ' Now behaviour alternates between seeking light and avoiding light '11Apr09 Painted Frea '10Apr09 remade inner toes, (got rid of lumps on toe ends), ' made toes longer, altered stepheightU and turnstride '09Apr09 Prog1 added MIN s_xxxxspeeds -s_xxxxspeeds to avoid going though zero ' Frea6 was a kicker because of this, interesting bug, looked like Frea was mad '***Frea7*** '09Apr09 Complete reshuffle into 3 pages, corrected all frame PAUSEs to 20ms '***Kicker*** ' has a bug in standonright AND standonleft (AND elswhere?) ' in standonright FOR S_stepspeed>4 ' LHat =LHat -S_stepspeed MIN LHwu goes through zero AND LHat=255 ' Frea kicks out sideways but otherwise seems TO walk ok. ' Interesting bug, Frea acts as IF she is mad at something. '***Frea6*** '08Apr09 rewired servo supply and fitted 5v 2A regulator to try and stop servo jitter, ' a lot turned out to be too short a pulse frame PAUSE 10, changed to PAUSE 20 ' Altered Toe springs, made ones with a single coil which improves springing '06Apr09 @$2D got eyeCortex working properly. [Something wrong with ServoV+ though] ' made LEDS indicate obstacle(green) and visionturn(red) '***Frea5*** '19Dec06 foot servos changed to new SD200, [one PB405 failed, wire fell off pot and gearbox tight] ' TellServos directions changed, copied to Prog1 ' [13Dec06]changing OR to AND worked, table edge behavior much better now '13Dec06 test at end of :setlegs changed to AND, think OR was left over from old toe switches ' f_Rfwd updated at end of first move in RT LT. Maybe this was why Frea sometimes walked off a table '29Oct06 tidied DoLoop and saved bytes. GreenLED resistor was 470R now 120R. '28Oct06 replaced broken hip servos with ankle servos (SD200) and fitted new ankle servos PB405 ' toe switches failed so fitted new ones which are closed for a hole, others were open for a hole. '21Oct06 moved Red led from off circuit board, now on flying leads, bell wire, above D9 '19Oct06 two series diodes put in servo supply, ' servos now run from ~5.3v off load, much smoother '***Frea4*** '17Sep06 a Servo made a gear slipping noise!!!! RightHip '17Sep06 sound names - description added, sound for toes changed '17Sep06 maxhappy was 40, now a variable set by Pot in DoLoop '14Sep06 PAUSE 100 added before IF Ltoe_=1 AND Rtoe_=1 THEN Upset0, helps stop toe switches bouncing '14Sep06 sRHmid CON 24'25, sLAmid CON 128'129 '6-9-05 Now working ok, pot and main and servo on/off switches put on ' Li battery installed - it is still charged from when Roger last used it, 12 months ago '26-8-05 split over two pages, renamed Frea2*_* and old version as Frea1*_* '***Frea3*** '25-8-05 if a toe doesn't make contact then MF turns towards the hole! ' 9,10 made 7,8 in revctrl, rightturn must follow a leftback '25-8-05 blew toe inputs, pins work as outputs so swapped 9/10, 13/14 '23-8-05 toe and tilt sensors were active high, now active low, ' pcb changed '23-8-05 got all the walking working with lots of min max and rewriting '21-8-05 tried some more at this pgm, 'sorted init and set but on entering mainloop it goes wild '15Oct04 Roger died '21Apr04 last dated code entry in 0-frea2.bsx (see below) '14/4/04 Frea2*** '22Mar04 start date of Roger's code 0-frea2.bsx (see below) '***Frea1*** '05Mar04 spent the day at Roger's, ' probably today I showed him Freya and that it could get up - he said "I'm going to make one" 'Notes '----- '1. in START Roger tested walklflag which had to be 1 which it always was as it was set just before ' walkflag was only 1 or 2 never 0 ' maybe he wanted to only stand up from fallen if walkflag<>0 ie walking ????? 'DB mods '------- 'new toe switches are pulled high, hole=low 'toe and tilt sensors were active high, now active low, pcb changed 'pgm space was to 25F 'PINS section added and names used instead of pin numbers 'CONS section added and names used instead of numbers 'loop: changed to MainLoop: 'the IF tests for gen1 and gen2 in vision meant that if gen1>250 then gen2 didn't get tested 'vision2: changed to v_seenobject: 'start: simplified ''and f_rightfwd=1' in fallen test removed, see Notes1 'Roger's code 'pulsout sLH,(850+(LHat*9)) 'Sx 0.8us => base pulse of 850 * 0.8 us =680us 'if svX=255 then svX *9 =2295 *0.8us =1836us 'so total pulse count = 850 +2295 = 3145 =2516us 'so servo pulse can vary from 680us to 2516us 'so 127 => 1594.4us 'my servo setup values, pulse varies from 500 to 2550us '10/0.8 =12.5 '255 * 0.8 * 10 = 2040 '600 * 0.8 = 480 '620 * 0.8 = 496 'so with base pulse of 620 and range of (0 - 255)*10 gives range of 500us - 2540us 'so 127 => 1890 => 1512us 'Rogers was better 10% more resolution 'Roger's notes 'start 21/3/04 start,loop,ramd_gen,sound1,servo added 'servo positions set for 5-250 sv count. '22/3/04 basics working started on servo motion - servo failure '23/3/04 servo repared basic walking action running, set legs almost works beeps to start 'if picked up beeeps and stops '24/3/04 leftview, rightview, sound3 added. setlegs modified '27/3/04 rand_control modified, left-right foot control working, right and left turns added '28/3/04 reverse and turn added '14/4/04 Frea rebuilt 11-13/4th fall over front and back tilt switch added, R eye emitter 'L eye LDR, Li pol battery. Save file as Frea2 '16/4/04 vision system added with random direction on hit detection + 4th sound routine '18/4/04 stand from lying on front and back added, start modified to look at tilt sensors. '21/4/04 stand from lying made conditional on leg sequence '--------------------------------------------------------------- 'CONS '---- 'VISION ROUTINES GreenOnTime CON 15 'roger=16 'illumination time for LDR eyescale CON 20 'roger=40 'scaler for LDR value eyemax CON 255 'roger=250 'max value for scaled LDR values eyedthresh CON 3 'roger=7 'threshold for object detection eyedeadband CON 10 '6" from 40W bulb ed=20, dark ed=>210 'Prog specific VARIABLES '----------------------- avoidctrl VAR Nib 'avoid control W_rand VAR Word 'random word variable W_LDReye VAR W_rand 'LDR RCTIMe variable W_tempPOT VAR W_rand 'POT scratchpad eyedark VAR Byte eyelit VAR Byte eyediff VAR Byte freq1 VAR eyedark 'FX_* routines freq2 VAR eyelit 'FX_* routines eyeRc VAR Byte 'eye R cortex eyeLc VAR Byte 'eye L cortex happy VAR Byte maxhappy VAR Byte lovelight VAR Byte 'max 32 'if >24 seek light else dark, if light <20 (ie bright) then /2 lovelightthresh CON 24 lovelightmax CON 32 toobright_ CON 20 'LDR dependent '============================================================== '****Common - copy this section into other progs*************** 'Common RAM '---------- 'CRAM_0 spare CRAM_cmnd CON 1 'PROG0 P0_done CON 1 'PROG1 FD CON 1 BK CON 2 TURN CON 3 RT CON 4 LT CON 5 STRETCH CON 6 'PROG2 setlegs CON 1 getup CON 2 CRAM_continuepoint CON 2 'pgm position to GOTO (not used) CRAM_S_speed CON 3 CRAM_flags CON 4 CRAM_LAat CON 5 CRAM_LHat CON 6 CRAM_RAat CON 7 CRAM_RHat CON 8 CRAM_happy CON 9 'P0 only CRAM_avoidctrl CON 10 'P0 only CRAM_lovelight CON 11 'P0 only CRAM63_thisprog CON 63 'Active prog run no '-------------------------------------------------------------- 'PINS '---- redled_ CON 0 'red led low is on greenled_ CON 1 'green eye led low is on whiskerLEDR PIN 2 'IR leds 1=on IR-LED + Yel-LED + 82R + 200Rpot in seriese whiskerLEDL PIN 3 'IR leds 1=on IR-LED + Yel-LED + 82R + 200Rpot in seriese eyepin CON 4 'LDR Light ->0 Dark ->255 spkr CON 5 'speaker whiskers_ PIN 6 'R L IR sensors in parallel, active low potpin CON 7 'pot 10k + 0u1 => 1 - 1609 s_LH CON 8 'left leg in+ out- Ltoe VAR IN9 'input from left foot 1=>activated s_LA CON 10 'servo left toe in+ out- Sx pin bad - won't input tipF_ VAR IN11 'input from fallen forwards switch tipB_ VAR IN12 'input from fallen backwards switch s_RA CON 13 'right toe in- out+ Sx pin bad - won't input Rtoe VAR IN14 'input from right foot 1=>activated s_RH CON 15 'right leg in- out+ f_flags VAR Byte f_Rfwd VAR f_flags.BIT0 '1 =>rightfootfwd f_RT VAR f_flags.BIT1 '1 =>RT set in TestToes:,TestWhiskers: only used in 1_TURN: f_stood VAR f_flags.BIT2 '1=>stood f_whiskerR VAR f_flags.BIT3 '1=>object detected f_whiskerL VAR f_flags.BIT4 '1=>object detected cmnd VAR Byte PROG VAR Byte '-------------------------------------------------------------- 'SERVOS '------ LHat VAR Byte'left leg up / down -ve in, +ve out LAat VAR Byte'left foot rotate -ve in, +ve out RAat VAR Byte'right foot rotate -ve in, +ve out RHat VAR Byte'right leg up / down -ve in, +ve out sp VAR Byte'servo pulse, counter - for/next i VAR sp 'Krellish: for/next IRcount servopulses VAR Byte'end of FOR NEXT KrellDuty VAR servopulses framepause CON 20 'pad servo frame pscaleSx CON 10 'servo pulse, 255*10*0.8= 2040us, so servo pulse is from ~0.5ms-2.5ms pscaleBS2 CON 4 'servo pulse, 255*4*2= 2040us pscaleBS2e CON 4 'servo pulse, 255*4*2= 2040us pscaleP CON 11 'servo pulse, 255*10.66*0.75= 2040us - not a good number!! alters all values '250*10*0.8=2000, 250*11*0.75=2062.5, 250*10*0.75=1875, 242.42*11*0.75=2000, ie 11=>+3% 'halfstride 20 =>21, stepheightU 20 =>21, stepheightD 25 =>26, turn 45 =>46 'probably not worth changing them pscale CON pscaleSx '1 logical count is 8us basepulseSx CON 62 'servo pulse,62*10*0.8=496us basepulseBS2 CON 62 'servo pulse,62*4*2=496us basepulseBS2e CON 62 'servo pulse,62*4*2=496us basepulseP CON 60 'servo pulse,60.12*11*0.75=496us, 60*11*0.75=495us, little difference basepulse CON basepulseSx stepheightU CON 20 'empirical, see at servos below, max before leg hits body stepheightD CON 35 'empirical, see at servos below, give a bit extra height halfstride CON 13 'SD200 'empirical stride CON halfstride +halfstride turnstride CON halfstride +15 'SD200 'empirical S_speed VAR Byte s_stepspeed VAR S_speed.LOWNIB 'servo, increment per frame, 6 goes over range, count goes wrong s_turnspeed VAR S_speed.HIGHNIB 'servo, increment per frame, 9 very snappy but Hg bounces in switches s_turnspeedf CON 5 'fast - servo, increment per frame, only used to rotate up foot in turn 'New servos Oct06 'PulseValue = memory +62 *10 620=>496uS 'uS = PulseValue *0.8 =>500 - 2500 'uS = memory +62 *10 *0.8 'memory = uS /0.8 /10 -62 or uS -500 /10 /0.8 so 127 =>1500 ' 'Servo uS 'RAin 2440 'not linear after 'RAmid 1440 'mid toes equal about CL 'RAout 560 'not linear after 'RHin 510 'leg hits body at 500 'RHmid 800 'feet flat =500+300 300=>38 'RHout 2450 'servo max =496+1954 1954=>244 'LAin 500 'not linear after 'LAmid 1480 'mid toes equal about CL 'LAout 2210 'not linear after 'LHin 2520 'servo max - no change if >, leg a little clear of body 'oldLHmid 2300 'feet flat =500+1810 1810=>226 'LHmid 2260 'feet flat =500+1760 1760/10=176 176/0.8=220 'LHout 560 'servo min =496+64 64=>8 556-8=248 'Actual servo byte values 'sRAin CON 3 sRAmid CON 118 '1440=>118 'sRAout CON 225 'sRHin CON 2 ? sRHmid CON 38 ' 38=>800 'sRHout CON 240 ? 'sLAout CON 24 sLAmid CON 123 '1480=>123 'sLAin CON 226 'sLHout CON ? sLHmid CON 220 ' 220=>2260 'sLHin CON ? 'Logical servo byte values - logical to physical conversion is done at pulsout time 'RAin con sRAin RAinstand CON 20 'Prog1 RAtbr CON RAmid -turnstride RAwbr CON RAmid -halfstride RAmid CON sRAmid RAwfr CON RAmid +halfstride RAoutstand CON 200 'Prog1 'RAout con 255 -sRAout 'RHin CON sRHin RHwu CON RHmid -stepheightU '38-20=18 RHmid CON sRHmid '38 RHwd CON RHmid +stepheightD '38+25 RHlieout CON 220 'RHout CON sRHout 'LAin con sLAin '254 LAinstand CON 20 'Prog1 LAtbl CON sLAmid -turnstride LAwbl CON LAmid -halfstride LAmid CON 255 -sLAmid LAwfl CON LAmid +halfstride LAoutstand CON 200 'Prog1 'LAout con sLAout '5 'LHin CON 255 -sLHin LHwu CON LHmid -stepheightU' '29-20=9 LHmid CON 255 -sLHmid '255-226=29 LHwd CON LHmid +stepheightD' '29+25 LHlieout CON 220 'LHout CON 255 -sLHout '****END common *********************************************** '============================================================== start: DEBUG CR,"P0" GET CRAM_flags,f_flags GET CRAM_cmnd,cmnd GET CRAM_LAat,LAat GET CRAM_LHat,LHat GET CRAM_RAat,RAat GET CRAM_RHat,RHat GET CRAM_happy,happy GET CRAM_S_speed,S_speed GET CRAM_lovelight,lovelight BRANCH cmnd,[init,DoStart] 'cmnd=0 if cold '-------------------------------------------------------------- 'Leaves Do_FD: cmnd =FD GOTO RUN1 Do_BK: cmnd =BK GOTO RUN1 Do_TURN: 'called from dornd: Avoid: depends on f_RT, only set in TestToes:,TestWhiskers: cmnd =TURN GOTO RUN1 Do_RT: GOSUB beep cmnd =RT GOTO RUN1 Do_LT: GOSUB beep :GOSUB beep cmnd =LT GOTO RUN1 'Do_STRETCH: 'done directly from QThenWhatToDo: GOSUB beep cmnd =STRETCH GOTO RUN1 Do_getup: cmnd =getup GOTO RUN2 'Do_setlegs: ' cmnd =setlegs ' GOTO RUN2 RUN1: PROG =1 GOTO RUNPROG RUN2: PROG =2 GOTO RUNPROG RUNPROG: DEBUG" C",DEC cmnd PUT CRAM_cmnd,cmnd PUT CRAM_flags,f_flags PUT CRAM_LAat,LAat PUT CRAM_LHat,LHat PUT CRAM_RAat,RAat PUT CRAM_RHat,RHat PUT CRAM_happy,happy PUT CRAM_avoidctrl,avoidctrl PUT CRAM_S_speed,S_speed PUT CRAM_lovelight,lovelight RUN PROG '-------------------------------------------------------------- init: DEBUG"I" LOW redled_ 'red led on to say we started avoidctrl =0 happy =0 s_stepspeed =2 s_turnspeed =2 f_whiskerR =0 f_whiskerL =0 'stand on both feet with toes to right LAat =LAwbl -8 'why -8?? toes hit otherwise!!! on final standup of init RAat =RAwfr LHat =LHmid RHat =RHmid eyeRc =0 'non valid value eyeLc =0 'non valid value lovelight =255 'GOTO Do_setlegs 'InitLegs: 'DEBUG"S" FOR sp =0 TO 30'30=some number 'Hips@=mid => feet flat on ground GOSUB TellHipServos PAUSE framepause NEXT 'DEBUG"S1" servopulses =(LHat -LHwu) MIN (RHwd -RHat) /S_stepspeed FOR sp =0 TO 30'servopulses 'stand on right LHat =LHat -S_stepspeed MIN LHwu RHat =RHat +S_stepspeed MAX RHwd GOSUB TellHipServos PAUSE framepause NEXT 'DEBUG"S2" 'DEBUG ?LAwbl, ?RAwfr FOR sp =1 TO 30 'rotate feet to Aat, give time for servos to get there GOSUB tellservos PAUSE framepause NEXT 'DEBUG ?LAat, ?RAat 'DEBUG"S3" servopulses =(LHmid -LHat) MIN (RHat -RHmid) /S_stepspeed FOR sp =0 TO servopulses 'left leg lower LHat =LHat +S_stepspeed MAX LHmid ' left leg out, RHat =RHat -S_stepspeed MIN RHmid ' right leg in GOSUB tellservos PAUSE framepause NEXT f_Rfwd =1 HIGH redled_ 'red led off initialisation finished '-------------------------------------------------------------- DoStart: 'Do one step or getup or LEDs or sound DEBUG CR,"Do" LOW redled_ 'blink red LED at start of every DoStart HIGH greenled_ 'green led off in case was on for Seen something lovelight =lovelight +1 MAX lovelightmax ReadPot: HIGH potpin 'discharge condenser PAUSE 1 'discharge time RCTIME potpin,1,W_tempPOT maxhappy =W_tempPOT /40 '0 - 37 happy =happy +1 MAX maxhappy s_stepspeed =happy/4 MIN 1 'speed=0 move never completes, so Frea hangs s_turnspeed =happy/4 MIN 1 'speed=0 move never completes, so Frea hangs DEBUG " H",DEC happy," sS",DEC s_stepspeed," sT",DEC s_turnspeed," ll",DEC lovelight HIGH redled_ 'end blink red LED at start of every DoStart QTipSensors: IF tipF_=0 OR tipB_=0 THEN Do_getup 'test if fallen QToes: 'if toe off then avoidctrl=1 f_RT =0=>avoid right 1=>avoid left 'so MF doesn't walk off a table 'DEBUG" Ltoe=",dec Ltoe," Rtoe=",dec Rtoe," f_Rfwd=",dec f_Rfwd,cr IF Ltoe=0 AND Rtoe=0 THEN Upset0 'both toes off _QavoidL: IF Ltoe=1 THEN _QavoidR 'not Ltoe off f_RT =1 avoidctrl =1 GOSUB snd2Ltoe DEBUG" hL" GOTO Avoid 'no point in QWhiskers _QavoidR: IF Rtoe=1 THEN QWhiskers 'not Rtoe off 'somehow toes are back down f_RT =0 avoidctrl =1 GOSUB snd1Rtoe DEBUG" hR" GOTO Avoid 'no point in QWhiskers 'GOTO Avoid QWhiskers: _irR: FREQOUT whiskerLEDR,1,38000 f_whiskerR =~whiskers_ 'R L sensors in parallel whiskerLEDR =f_whiskerR OUTPUT whiskerLEDR 'FREQOUT leaves it INPUT IF f_whiskerR THEN f_RT =0 'turn away from Right object avoidctrl =1 ENDIF PAUSE 20 'otherwise sensor doesn't work _irL: FREQOUT whiskerLEDL,1,38000 f_whiskerL =~whiskers_ 'R L sensors in parallel whiskerLEDL =f_whiskerL OUTPUT whiskerLEDL 'FREQOUT leaves it INPUT IF f_whiskerL THEN f_RT =1 'turn away from Right object avoidctrl =1 ENDIF 'GOTO Dostart 'DEBUG " ir",BIN1 f_whiskerL, BIN1 f_whiskerR, CR 'PAUSE 100 'GOTO QWhiskers IF avoidctrl=0 THEN Explore DEBUG " ir",BIN1 f_whiskerL, BIN1 f_whiskerR Avoid: 'DEBUg?tavoidctrl 'avoidctrl=1 entry point fot QToes->Avoid 'avoidctrl=1 entry point fot QWhiskers->Avoid happy =happy /2 BRANCH avoidctrl,[Explore,_Bk,_Bk,_Turn] 'and fall through when done avoidctrl =0 GOTO DoStart _Bk: avoidctrl =avoidctrl +1 'avoiding so point to next step back GOTO Do_BK _Turn: avoidctrl =avoidctrl +1 'avoiding so point to next step back GOTO Do_TURN '-------------------------------------------------------------- Upset0: 'If both feet off beep and flash DEBUG" Upset",CR happy =happy /2 DEBUG?Ltoe,?Rtoe IF Ltoe=1 AND Rtoe=1 THEN DoStart 'if both feet down then MainLoop LOW redled_ 'red led on GOSUB snd3upset HIGH redled_ 'red led off IF Ltoe=0 AND Rtoe=0 THEN QThenWhatToDo 'both toes off GOTO DoStart QThenWhatToDo: 'W_rand is local to QThenWhatToDo: & doPause: RANDOM W_rand IF W_rand.HIGHBYTE <80 THEN DoPause 'pause ~1/3 of the time GOSUB SFX_17help PROG =1 :cmnd =STRETCH :GOTO RUNPROG doPause: GOSUB SFX_4unsure PAUSE W_rand.LOWBYTE *32 'upto ~8secs GOTO DoStart '-------------------------------------------------------------- Explore: 'dovision - if object seen Do_LT or Do_RT 'dornd 'DoStart dovision: 'W_LDReye is local to dovision: DEBUG" V" HIGH eyepin 'discharge condenser PAUSE 10 'discharge time RCTIME eyepin,1,W_LDReye 'time condenser charge (read LDR) eyedark =W_LDReye /eyescale MAX eyemax 'scale result and max it LOW greenled_ 'green led on PAUSE GreenOnTime 'pause to allow LDR to register HIGH eyepin 'discharge condenser PAUSE 10 'discharge time RCTIME eyepin,1,W_LDReye 'read LDR + green LED HIGH greenled_ 'green led off eyelit =W_LDReye /eyescale MAX eyedark 'scale result and max it, avoid negatives for eyediff eyediff =eyedark -eyelit DEBUG" ed",DEC eyedark,",el",DEC eyelit,",e~",DEC eyediff,"|",DEC eyedthresh IF eyedifftoobright_ THEN _vCget 'brighter ->0 lovelight =lovelight /2 '+ve _vCget: IF f_Rfwd=1 THEN _vL _vR: eyeRc =eyedark GOTO _vCgot _vL: eyeLc =eyedark _vCgot: dornd: 'DEBUG"rnd" RANDOM W_rand IF W_rand.LOWBYTE<100 THEN SeekLight '100 or SeekDark: depending on lovelight IF W_rand.LOWBYTE<200 THEN Do_FD '100 IF W_rand.LOWBYTE<215 THEN Do_TURN ' 15 depends on f_RT, only changed in TestToes: =>memory of bad IF W_rand.LOWBYTE<225 THEN Do_RT ' 5 so sometimes will turn to bad side IF W_rand.LOWBYTE<225 THEN Do_LT ' 5 so sometimes will turn to bad side IF W_rand.LOWBYTE<230 THEN Do_BK ' 5 IF W_rand.LOWBYTE<240 THEN Krellish ' 10 do nothing 'else ' 15 GOSUB R_Squak3 'this is my space but I am not threatening you GOTO DoStart '-------------------------------------------------------------- SeekLight: DEBUG " eL",DEC eyeLc,",eR",DEC eyeRc,"|",DEC eyedeadband," ",DEC f_Rfwd IF (eyeRc AND eyeLc) =0 THEN Do_FD 'skip until valid values for both IF lovelight (eyeLc+eyedeadband) THEN _vLT IF eyeLc>(eyeRc+eyedeadband) THEN _vRT GOTO dornd SeekDark: IF eyeRc>(eyeLc+eyedeadband) THEN _vRTd IF eyeLc>(eyeRc+eyedeadband) THEN _vLTd GOTO dornd _vLTd: 'vision LeftTurn dark LOW greenled_ 'greenLED + redLED on indicates Dark turn _vLT: DEBUG" vL" LOW redled_ 'indicate Light turn eyeLc =0 'discard turn values eyeRc =0 GOTO Do_LT _vRTd: 'vision RightTurn dark LOW greenled_ 'greenLED + redLED on indicates Dark turn _vRT: DEBUG" vR" LOW redled_ 'indicate Light turn eyeLc =0 'discard turn values eyeRc =0 GOTO Do_RT '-------------------------------------------------------------- 'SOUND ROUTINES beep: 'LT:,RT:,Stretch: FREQOUT spkr,400,500 :RETURN snd1Rtoe: 'TestToes/_avoidR 'debug" snd1" FREQOUT spkr,200,1100,1000 RETURN snd2Ltoe: 'TestToes/_avoidL 'debug" snd3" FREQOUT spkr,200,1600,1500 RETURN snd3upset: 'Upset0: 'debug" snd3" FREQOUT spkr,30,2000 PAUSE 10 FREQOUT spkr,30,2000 RETURN sound4seen: 'v_seenobject: 'debug" snd4" FREQOUT spkr,80,2700 RETURN SFX_4unsure: 'bzz (soft) 'unsure of this' 'Upset0/doPause: FOR freq1 = 10 TO 50 STEP 10 FOR freq2 = 50 TO 10 STEP 50 FREQOUT spkr, 15, (freq2 * 20) NEXT NEXT RETURN R_Squak3: 'rnd6: 'this is my space but I am not threatening you' GOSUB SFX_10 GOSUB SFX_7 RETURN SFX_7: 'R_Squak3: '7 twwwweeeaeat 'this is my space' FOR freq1 = 30 TO 70 STEP 5 FOR freq2 = 70 TO 30 STEP 5 FREQOUT spkr, 10, (freq1 * freq2) NEXT NEXT RETURN SFX_10: 'R_Squak3: '10 'phone' (buzz soft) FOR freq1 = 1 TO 30 FREQOUT spkr, 20, ((freq1 * 14) + 450) NEXT RETURN SFX_17help: 'QThenWhatToDo: '17 aeaeaeh 'help' FOR freq1 = 0 TO 150 STEP 10 FREQOUT spkr, 20, 1295 - freq1 FREQOUT spkr, 20, 1095 + freq1 NEXT RETURN '-------------------------------------------------------------- Krellish: 'redled_ CON 0 'red led low is on 'greenled_ CON 1 'green eye led low is on cmnd =W_rand.LOWBYTE/25 FOR cmnd =cmnd TO 0 FOR i =0 TO 30 LOW 0 HIGH 1 PAUSE 30-i HIGH 0 LOW 1 PAUSE i NEXT PAUSE 1 FOR i =0 TO 30 HIGH 0 'off LOW 1 'on PAUSE 30-i LOW 0 HIGH 1 PAUSE i NEXT NEXT GOTO DoStart '-------------------------------------------------------------- '****copy this section into other progs using the servos**** TellServos: 'convert logical direction to physical PULSOUT s_LA,basepulse +255 -LAat *pscale 'left foot rotate -out / +in PULSOUT s_RA,basepulse +RAat *pscale 'right foot rotate +out / -in TellHipServos: PULSOUT s_LH,basepulse +255 -LHat *pscale 'left leg +in / -out PULSOUT s_RH,basepulse +RHat *pscale 'right leg -in / +out 'DEBUG DEC (basepulse +255 -LHat *8)," " 'DEBUG DEC (basepulse +RHat *8),CR RETURN '--------------------------------------------------------------