; Self-Wiring Array of Bicores ; Parallax assembly for the PicBot II robot platform ; Modified August 15 Copyright 1999 Terry Newton ; Modified November 23 2006 - added watch code ; Modified November 28 2006 - IR obstacle detection... ; This requires the addition of a pair of IR LEDs and circuitry ; to drive them to pins RB6/RB7 (also the programming pins) ; RB6 drives the right LED when high, RB7 drives the left LED. ; A differential drive method is used to null supply V difference. ; The following circuit (x2) was used to avoid loading the pins... ; ; ^^ IR led ; RB6/RB7 b c.---470---|<|----< + ; >---82K--*----|< bcx70k ; | e| (hi gain npn) ; `-82K--*---< ground ; ; To the rest of the program, an IR hit is the same as a feeler hit, ; a lot of logic would need rearranging to actually use as separate ; senses. This version tests the idea but isn't an ideal implementation. ; IR senses are stored in bits leftir and rightir of irsenses, so to ; make it just a little fairer to the robot, IR hits are not considered ; "bad" like feeler hits. ; ; ; This program includes a version of the Nervous Network, ; a robotic control structure invented and internationally ; patented by Mark Tilden. Commercial use of this program ; or the architecture it simulates is prohibited without ; arrangement with the patent holder. ; ; Simulated here is an array of 5 bi-cores, or two-neuron ; nervous networks that serve as complementary oscillators. ; 1 bicore is hardcoded to L/R light levels, 4 bicores have ; configurable influence sources and weights, the last one ; is hardcoded to differentiators which drive the motors. ; Each configurable bicore has 2 inputs w/ weights per node. ; Processed sensory bits are also available for influece. ; Input to bicore nodes is integrated, weight determines ; the ramp speed. Feelers are connected using Mark Tilden's ; XOR method (as in the "Beam Ant") however they can be ; ignored to allow escape from tight places. Differentiator ; values and motor-speed bits are also configurable. ; ; To aquire suitable connections, scoring is done by how ; much the feelers touch. Each time they are not touching, ; score is increased by 1. When a feeler touches, score is ; decreased by a "punishment" value. If score reaches max, ; the network configuration is saved to 1 of 3 randomly ; chosen eeprom slots. If the score drops below a threshold, ; random changes are made to the configuration. If it continues ; to drop, a network is restored from eeprom. If it still keeps ; scoring poorly, the entire configuration is randomised. ; Aprox. 1 in 50 chance of changing something anyway. ; DEVICE PIC16F84, RC_OSC, WDT_ON, PWRT_ON include 'd:\dos\pictools\16f84.inc' fuses _RC_OSC fuses _WDT_ON fuses _PWRTE_ON ; simulation/robot constants outercycles = 12 innercycles = 30 pcorebase = 120 levelbase = 55 maxconnect = 50 diffminimum = 30 ; evolution constants startingscore = 30 maxscore = 75 changescore = 24 punish1 = 4 punish2 = 10 punishment = 10 reward1 = 1 reward2 = 4 lightreward = 2 lightpunish = 2 ; sensor constants (mod 12/19/06) ; lightl/lightr range from 0 (totally dark) to 127 (totally light) darkthresh = 1 ; below this level does not distinguish L/R phdiffmask = 01111110b ;when detecting L/R, 'and' by this (dead zone) irbright = 30 ; if light <= irbrite then IR code uses irdimdiff = 2 ; irdimdiff to determine if light increased irbrightdiff = 3 ; in bright light uses irbrightdiff instead phdelay = 3 ; how long ph caps are shorted phrange = 0 ; 0-6, smaller = optimize for dimmer light motordelay = 125 ; motor delay (ms/2, delay called twice) ; control constants optsleep = 10001111b ; pin names rightfeelpin = ra.0 rightphotopin = ra.1 leftphotopin = ra.2 leftfeelpin = ra.3 powerpin = rb.0 statusled = rb.1 rightirled = rb.6 leftirled = rb.7 ; rb.2 = left motor + (rev) ; rb.3 = left motor - (for) ; rb.4 = right motor - (rev) ; rb.5 = right motor + (for) firstrun = status.3 flags = 12 badenv = flags.0 badmove = flags.1 lastbad = flags.2 lastbad2 = flags.3 environment = 13 leftfeeler = environment.0 rightfeeler = environment.1 lightonleft = environment.2 lightonright = environment.3 action = 14 milliseconds = 15 outerloop = 16 innerloop = 17 lightl = 18 lightr = 19 temp = 20 temp1 = 21 temp2 = 22 ; end control code vars, start behavior vars... ; define network (these must be in sequence, 17 bytes) connecta = 23 connectb = 27 weighta = 31 weightb = 35 motorcon = 39 ; connection sources... (2 per byte for connectA and connectB) ; 0 = always 0 ; 1 = left feeler ; 2 = right feeler ; 3 = light on left ; 4 = light on right ; 5 = photo-bicore A (left) ; 6 = photo-bicore B (right) ; 7 = badenv flag ; 8/9 = bicore #0 outs A/B ; 10/11 = bicore #1 outs A/B ; 12/13 = bicore #2 outs A/B ; 14/15 = bicore #3 outs A/B (motor) uselfeel = motorcon.7 userfeel = motorcon.6 actlrev = motorcon.5 actrrev = motorcon.4 reversebeef = motorcon.4 reverseflip = motorcon.3 ; bits 0-2 = output diffs = value * 4 + min score = 40 bigscore = 41 ; bicore variables inputa = 42 inputb = 46 levela = 50 levelb = 54 states = 58 phstates = 59 pha = phstates.0 phb = phstates.1 tempstates = 60 diffa = 61 diffb = 62 phlevela = 63 phlevelb = 64 i = 65 j = 66 k = 67 tempbit = flags.7 dirtyram = flags.6 changeat = 68 ; to fix a flaw... lightsave = 69 ; save light level for training ;added... oldenvironment = 70 ltemp = 71 rtemp = 72 irsenses = 73 leftir = irsenses.4 rightir = irsenses.5 ircomp = 74 ; start of program flow ; --------------------- reset mov !option, #optsleep clrb rp0 clrb intcon mov !ra, #00011111b mov !rb, #11000001b clr ra clr rb jnb firstrun, mainloop clr flags mov changeat, #changescore mov score, #startingscore mov bigscore, #startingscore ; new watch code... mainloop jb powerpin, watch doze sleep jmp reset watch jb badmove, wakeup ;don't doze if learning mov oldenvironment, environment call readsenses cjne environment, oldenvironment, wakeup jmp doze wakeup ; behavior code here ; ------------------------------------------------------ ; ensure all states are complementary and phstates, #3 cje phstates, #0, fixstates cje phstates, #3, fixstates mov temp1, states mov i, #4 chkstloop mov temp, temp1 and temp, #3 cje temp1, #0, fixstates cje temp1, #3, fixstates rr temp1 rr temp1 djnz i, chkstloop jmp statesok fixstates mov phstates, #1 mov states, #01010101b statesok mov k, #outercycles outer_loop ; obstacle detection... mov !rb, #00000001b ;make ir led pins outputs setb leftirled ;turn on left led clrb rightirled ;turn off right led call readsenses ;check light level mov ltemp, lightl ;save for comparison mov rtemp, lightr clrb leftirled ;turn off left led setb rightirled ;turn on right led call readsenses ;check light level clrb rightirled ;turn off right led mov !rb, #11000001b ;make ir led pins inputs/hi-z clr irsenses mov ircomp, #irdimdiff ;default dim threshold (min difference) cjb lightr, irbright, od2 ;jump if light (r) below irbright ; has to be a jump since mov is really 2 instructions mov ircomp, #irbrightdiff ;use bright diff if light >= irbright od2 sub ltemp, lightl ;subtract lightL from prev. Ltemp sub lightr, rtemp ;subtract prev. Rtemp from lightR csb ltemp, ircomp ;skip if Ltemp < threshold setb leftir ;ir hit on left if Ltemp >= threshold csb lightr, ircomp ;skip if lightR < threshold setb rightir ;ir hit on right if lightR >= threshold ; read senses into environment bits, lightL/R call readsenses call evalfeelers ;removed from readsenses sub to ;rotate bad flags just once ;feeler "badness" is evaluated before adding IR hits to the feeler ;senses, so they invoke the same response but do not count against ;the current network's goodness score, i.e. IR hits don't hurt. jnb leftir, nlir setb leftfeeler setb statusled ; nop nlir jnb rightir, nrir setb rightfeeler setb statusled ; nop nrir ; scoring function... jnb badenv, scoregood jb lastbad, scorebad jb lastbad2, scorebad sub score, #punish1 jmp scoredone scorebad sub score, #punish2 jmp scoredone scoregood jnb lastbad, scoresome add score, #reward1 jmp scoredone scoresome jnb lastbad2, scorelittle add score, #reward2 skip scorelittle inc score scoredone ; reward if light is getting brighter, punish if darker mov temp, lightL add temp, lightR and temp, #11111000b cjbe temp, lightsave, rewardl1 add score, #lightreward rewardl1 cjae temp, lightsave, rewardl2 sub score, #lightpunish rewardl2 mov lightsave, temp ; normalise score fixscore cjb score, #maxscore, fixscore2 mov score, #maxscore fixscore2 snb score.7 clr score ; if all clear, set usexfeel's 1 and actxrev's to 0 jb badenv, resetbits1 jb lastbad, resetbits1 jb lastbad2, resetbits1 setb uselfeel setb userfeel clrb actlrev clrb actrrev resetbits1 ; self-programming triggers jb score.7, restorenet ; restore if < 0 cjb score, changeat, changenet ; change if < change cje score, #maxscore, savenet ; save if = max ; every now and then change something call getrndbyte cjb temp, #5, changenet ; approx 1 in 50 chance resumeloop ; reset change threshold if doing ok cjbe score, #changescore, rct_done mov changeat, #changescore rct_done ; run network and move robot.... mov j, #innercycles inner_loop setb badmove ;forward movement clears ; do photo bi-core mov temp2, phstates ;pha,phb in bits 0,1 cje phlevela, #0, pbc_trigA snb phb dec phlevela jmp pbc_endA pbc_trigA setb temp2.0 clrb temp2.1 mov phlevela, #pcorebase sub phlevela, lightl pbc_endA cje phlevelb, #0, pbc_trigB snb pha dec phlevelb jmp pbc_endB pbc_trigB setb temp2.1 clrb temp2.0 mov phlevelb, #pcorebase sub phlevelb, lightr pbc_endB mov phstates, temp2 ;update changes ; do bicores 0 to 3 mov tempstates, states clr i dobcloop mov 4, #levela add 4, i mov temp, 0 cje temp, #0, abc_trigA cjne i, #0, abc_A1 movb tempbit, states.1 abc_A1 cjne i, #1, abc_A2 movb tempbit, states.3 abc_A2 cjne i, #2, abc_A3 movb tempbit, states.5 abc_A3 cjne i, #3, abc_A4 movb tempbit, states.7 abc_A4 snb tempbit dec temp jmp abc_endA abc_trigA cjne i, #0, abc_A5 setb tempstates.0 clrb tempstates.1 abc_A5 cjne i, #1, abc_A6 setb tempstates.2 clrb tempstates.3 abc_A6 cjne i, #2, abc_A7 setb tempstates.4 clrb tempstates.5 abc_A7 cjne i, #3, abc_A8 setb tempstates.6 clrb tempstates.7 abc_A8 mov 4, #inputa add 4, i mov temp1, 0 mov temp, #levelbase sub temp, temp1 cjne i, #3, abc_endA call getdiffbits mov diffa, temp1 clr diffb abc_endA mov 4, #levela add 4, i mov 0, temp mov 4, #levelb add 4, i mov temp, 0 cje temp, #0, abc_trigB cjne i, #0, abc_B1 movb tempbit, states.0 abc_B1 cjne i, #1, abc_B2 movb tempbit, states.2 abc_B2 cjne i, #2, abc_B3 movb tempbit, states.4 abc_B3 cjne i, #3, abc_B4 movb tempbit, states.6 abc_B4 snb tempbit dec temp jmp abc_endB abc_trigB cjne i, #0, abc_B5 setb tempstates.1 clrb tempstates.0 abc_B5 cjne i, #1, abc_B6 setb tempstates.3 clrb tempstates.2 abc_B6 cjne i, #2, abc_B7 setb tempstates.5 clrb tempstates.4 abc_B7 cjne i, #3, abc_B8 setb tempstates.7 clrb tempstates.6 abc_B8 mov 4, #inputb add 4, i mov temp1, 0 mov temp, #levelbase sub temp, temp1 cjne i, #3, abc_endB call getdiffbits mov diffb, temp1 clr diffa abc_endB mov 4, #levelb add 4, i mov 0, temp ; end loop inc i cjb i, #4, dobcloop mov states, tempstates ; transfer states to inputs according to ; connection and connection weight arrays clr i dowireloop mov 4, #connecta add 4, i mov temp, 0 and temp, #15 call getactivation mov 4, #weighta add 4, i mov temp, 0 and temp, #15 mov 4, #inputa add 4, i mov temp1, 0 jnb tempbit, ts_notsetA add temp1, temp cjbe temp1, #maxconnect, ts_endA mov temp1, #maxconnect jmp ts_endA ts_notsetA sub temp1, temp snb temp1.7 clr temp1 ts_endA mov 4, #inputa add 4, i mov 0, temp1 mov 4, #connectb add 4, i mov temp, 0 swapf temp,1 and temp, #15 call getactivation mov 4, #weightb add 4, i mov temp, 0 mov 4, #inputb add 4, i mov temp1, 0 swapf temp,1 and temp, #15 jnb tempbit, ts_notsetB add temp1, temp cjbe temp1, #maxconnect, ts_endB mov temp1, #maxconnect jmp ts_endB ts_notsetB sub temp1, temp snb temp1.7 clr temp1 ts_endB mov 4, #inputb add 4, i mov 0, temp1 ; end loop inc i cjb i, #4, dowireloop ; motor drive clr action cje diffa, #0, mot_offA dec diffa setb action.3 mot_offA cje diffb, #0, mot_offB dec diffb setb action.5 mot_offB ; connect feelers to other side of motors clrb tempbit jnb uselfeel, mot_dofeelB jnb leftfeeler, mot_dofeelB setb action.4 setb tempbit mot_dofeelB jnb userfeel, mot_endfeel jnb rightfeeler, mot_endfeel setb action.2 setb tempbit mot_endfeel ; reverse motor if associated actxrev bit set jnb actlrev, mot_rev1 setb action.2 setb tempbit mot_rev1 jnb actrrev, mot_rev2 setb action.4 setb tempbit mot_rev2 ; if reverse and flip bit set then ; connect drive bits to inverse of bc outs jnb tempbit, mot_flipend jnb reverseflip, mot_flipend movb action.3, /states.6 movb action.5, /states.7 mot_flipend ; smoke patrol jnb action.2, mot_sp1 jnb action.3, mot_sp1 clrb action.2 clrb action.3 mot_sp1 jnb action.4, mot_sp2 jnb action.5, mot_sp2 clrb action.4 clrb action.5 mot_sp2 ; drive motor ; (ignore beef bits, tired of slow speed...) mov rb, action ; jb rb.3, clearbad ;moving.. ; jb rb.5, clearbad ;moving ; jmp go_on ;clearbad clrb badmove ;go_on ; with IR detection this isn't fair, penalizing for "proper" ; behavior, leading to evolving nets that stop one motor in ; response to obstacles, so that forward motion can be ; maintained. Therefore, now only no motion at all is "bad"... mov temp, action and temp, #00111100b ;look at just motor bits sz ;skip if zero (no motion) clrb badmove ;if any motion clear badmove flag ; smoke-patrol already zeroed any '11' pairs, so any bit set is motion ; ; done with microcycle! finally! clr wdt ; prevent timeout djnz j, inner_loop clr rb djnz k, outer_loop jmp exit ; subroutine for mapping connection numbers getactivation clrb tempbit cjne temp, #1, map_1 snb leftfeeler setb tempbit map_1 cjne temp, #2, map_2 snb rightfeeler setb tempbit map_2 cjne temp, #3, map_3 snb lightonleft setb tempbit map_3 cjne temp, #4, map_4 snb lightonright setb tempbit map_4 cjne temp, #5, map_5 snb pha setb tempbit map_5 cjne temp, #6, map_6 snb phb setb tempbit map_6 cjne temp, #7, map_7 snb badenv setb tempbit map_7 cjne temp, #8, map_8 snb states.0 setb tempbit map_8 cjne temp, #9, map_9 snb states.1 setb tempbit map_9 cjne temp, #10, map_10 snb states.2 setb tempbit map_10 cjne temp, #11, map_11 snb states.3 setb tempbit map_11 cjne temp, #12, map_12 snb states.4 setb tempbit map_12 cjne temp, #13, map_13 snb states.5 setb tempbit map_13 cjne temp, #14, map_14 snb states.6 setb tempbit map_14 cjne temp, #15, map_15 snb states.7 setb tempbit map_15 ret ; good net, save to eeprom savenet jnb dirtyram, resumeloop call getrndee clr i saveloop mov 4, #connecta add 4, i mov eedata, 0 call writememory inc eeadr inc i cjb i, #17, saveloop clrb dirtyram inc bigscore cjbe bigscore, #maxscore, resumeloop mov bigscore, #maxscore mov score, #startingscore ; make it work for it jmp resumeloop ; get random eeprom start, 0, 20 or 40 getrndee call readsenses mov temp2, tmr0 and temp2, #00000110b clr eeadr cje temp2, #00000110b, getrndee cjne temp2, #00000010b, rndee_1 mov eeadr, #20 rndee_1 cjne temp2, #00000100b, rndee_2 mov eeadr, #40 rndee_2 ret ; restore net from eeprom restorenet call getrndee clr i restoreloop call readmemory mov 4, #connecta add 4, i mov 0, eedata inc eeadr inc i cjb i, #17, restoreloop clrb dirtyram mov score, #startingscore mov changeat, #changescore sub bigscore, #punishment jnb bigscore.7, resumeloop ; randomise everything clr i shakeloop call getrndbyte mov 4, #connecta add 4, i mov 0, temp inc i cjb i, #17, shakeloop mov bigscore, #startingscore jmp resumeloop ; make random changes changenet setb dirtyram mov changeat, score ; move threshold down to avoid ; more changes unless really worse picknum call readsenses mov i, tmr0 and i, #00011110b clc rr i call getrndbyte mov 4, #connecta add 4, i mov temp1, 0 jnb tmr0.5, chgnet_1 and temp, #11110000b and temp1, #00001111b jmp chgnet_2 chgnet_1 and temp1, #11110000b and temp, #00001111b chgnet_2 or temp, temp1 mov 4, #connecta add 4, i mov 0, temp ; randomly change motor con bits call getrndbyte mov temp1, temp jnb temp1.0, chgnet_3 jnb temp1.1, chgnet_3 call getrndbyte and temp, #00000111b and motorcon, #11111000b or motorcon, temp chgnet_3 jnb temp1.2, chgnet_4 jnb temp1.3, chgnet_4 call getrndbyte and temp, #00111000b and motorcon, #11000111b or motorcon, temp chgnet_4 jnb temp1.4, resumeloop jnb temp1.5, resumeloop call getrndbyte and temp, #11000000b and motorcon, #00111111b or motorcon, temp jmp resumeloop ; get random byte in temp getrndbyte call readsenses mov temp2, tmr0 and temp2, #00011110b clc rr temp2 call readsenses mov temp, tmr0 and temp, #00011110b rl temp rl temp rl temp and temp, #11110000b or temp, temp2 ret ; get value for differentiators getdiffbits mov temp1, motorcon and temp1, #7 clc rl temp1 clc rl temp1 add temp1, #diffminimum ret exit clrb statusled ; ------------------------------------------------------ ; end behavior code mov milliseconds, #motordelay call delay call delay jmp mainloop ; time delay subroutine delay mov outerloop, milliseconds delay_outer mov innerloop, #60 delay_inner nop nop clr wdt djnz innerloop, delay_inner djnz outerloop, delay_outer ret ; read senses into environment readsenses clr environment movb leftfeeler, /leftfeelpin movb rightfeeler, /rightfeelpin ; read left photocell ; make ra.2 output mov !ra, #00011011b clr ra mov milliseconds, #phdelay call delay mov !ra, #00011111b mov lightl, #127 photo_loopL jb leftphotopin, photo2 jmp $+phrange+1 jmp $+1 jmp $+1 jmp $+1 jmp $+1 jmp $+1 jmp $+1 djnz lightl, photo_loopL photo2 ; make ra.1 output mov !ra, #00011101b clr ra mov milliseconds, #phdelay call delay mov !ra, #00011111b mov lightr, #127 photo_loopR jb rightphotopin, photo3 jmp $+phrange+1 jmp $+1 jmp $+1 jmp $+1 jmp $+1 jmp $+1 jmp $+1 djnz lightr, photo_loopR photo3 ;modified this part so lightl/lightr not affected by phdiffmask mov temp1, lightl mov temp2, lightr and temp1, #phdiffmask and temp2, #phdiffmask csbe temp1, temp2 setb lightonleft csbe temp2, temp1 setb lightonright cjne temp1, temp2, photo4 cjbe lightl, #darkthresh, photo4 setb lightonleft setb lightonright photo4 ret evalfeelers ;made this a sub so above can be called multiple times ; rotate flags and set badenv if feelers touching movb lastbad2, lastbad movb lastbad, badenv setb badenv jb leftfeeler, read2 jb rightfeeler, read2 clrb badenv read2 ret ; read from eeprom memory readmemory setb rp0 setb eecon1.0 clrb rp0 ret writememory and eeadr, #00111111b setb rp0 bsf eecon1, 2 movlw 55h movwf eecon2 movlw 0AAh movwf eecon2 bsf eecon1, 1 eewait jb eecon1.1, eewait clrb eecon1.5 clrb rp0 ret retw '(c)06WTN' ; end of source