; Micro-Controller BRAIN for robotic control ; by Terry Newton Last Modified 11/15/96 ; PIC16C5x source code for Parallax assembler ; (fits on a PIC16C54 with room to spare) device PIC16C56, RC_OSC, WDT_OFF, PROTECT_OFF reset startup ; Robot variables... EnvPort = 06h ; port B ; motor/relay drive bits... L_drive = ra.0 ; pulse-width modulated to R_drive = ra.1 ; control motor speed Reverse = rb.7 ; relay to reverse one motor PortAMask = 00001000b ; bit 0-2 outputs bit 3 input PortBMask = 01111111b ; bits 0-6 inputs bit 7 output ThisEnv = 15h LastEnv = 16h Action = 17h PrevAct = 18h Confidence = 19h ConfMask = 00000011b ; confidence level 0 to 3 ActionMask = 11111000b ; action bits 3-7 MotorL1 = 7 ; define Action (motor) bits... MotorL2 = 6 MotorR1 = 5 MotorR2 = 4 MotorRev = 3 ; acceptable 'normal' output states... Goal0 = 10100000b ; forward slow Goal1 = 01010000b ; forward medium Goal2 = 11110000b ; forward fast Goal3 = 11010000b ; fast forward and right Goal4 = 01110000b ; fast forward and left Goal5 = 11100000b ; turn right Goal6 = 10110000b ; turn left ; environment bits... L_Shadow = 0 ; high when shadow is on the left R_Shadow = 1 ; high when shadow is on the right L_Stall = 2 ; high when motor is drawing excessive R_Stall = 3 ; current indicating a stall condition IR_Obj = 4 ; low when IR detects an object ahead Feeler = 5 ; Forward-facing touch switch (added 11-6) Sense6 = 6 ; (extra) active low, doubles as LED out EnvMask = 01111111b ; mask of available environment bits ALMask = 01010000b ; mask of active-low environment bits ; unacceptable environment bits... InhibA = 00111100b ; IR, stall or feel - for normal conditions ; unacceptable environment after so many bad moves... InhibB = 00001111b ; stall or photo - plan B (go to the light) Flags = 1Ah Happy = Flags.0 ; happy if set, sad if not HitMax = Flags.1 ; confidence increased to maxconfidence DipMax = Flags.2 ; confidence decreased from maxconfidence Boredom = 1Bh ; boring move counter BoreFactor = 8 ; bored after 8 identical moves with no stimulus PFCW = 1Ch ; Program Flow Control Word ThisLast = PFCW.0 ; Use This+Last addressing UseGamma = PFCW.1 ; Use Gamma routines UseForget = PFCW.2 ; Use Forget routines RMStart = PFCW.3 ; Start random move at conf=1 GMinc2 = PFCW.4 ; Increment good move by 2 GetBored = PFCW.5 ; Use boredom counter if set LockPFCW = PFCW.6 ; lock the PFCW if set (no self mod) InitialPFCW = 00000000b ; initial PFCW starting value ModPFMask = 00111111b ; mask of bits creature can self-modify BadMoves = 1Dh ; counter for algorthm mods and core-wipe ModBit = 3 ; modify PFCW after bit3 of badmoves goes high ModInhib = 2 ; go to InhibB after bit2 goes high MaxBadMoves = 100 ; consecutive bad moves before wipe LockNoBad = Flags.7 ; one good move locks out wipe Temp = 1Eh PlanBcount = 1Fh ; counter for plan-b inhibit mask NumPlanBs = 5 ; execute plan b for 5 moves when engaged Bflag = Flags.6 ; plan-b flag - selects InhibA or InhibB ; note - when Bflag is set the associations are stored to different ; eeprom addresses by rbyte and wbyte (bit 7 of addr forced high) StatusLED = rb.6 StatusMask = 00111111b ; make bit 6 an output StatusXOR = 01000000b ; for flipping LED bit DefAddr1 = 00011111b ; top of 8Kbyte EEPROM org 000h ; (all called subroutines must fall in page 0, so this stuff's first) ; EEPROM routines modified from Microchip application note AN558 ; bitin and bitout routines are coded in-line to permit write and ; read routines to be called without overflowing stack. Also modified ; some of the timing routines and dropped several nops. Using with ; RC osc. 4.7K and 27pF Delays are approximate and not calibrated! ; Any bugs are my own (the following code is in Microchip format) ; EEPROM variables... (EE data = RA.3, clock = RA.2) statby = 03h eeport = 05h ; port A eeprom = 0Ah bycnt = 0Bh addr = 0Ch addr1 = 0Dh datai = 0Eh datao = 0Fh txbuf = 10h count = 11h bcount = 12h loops = 13h loops2 = 14h di = 3 do = 2 sdata = 3 sclk = 2 outmask = 00000000b inmask = 00001000b ; wait routine - waits approx loops milliseconds wait movlw 150 movwf loops2 wait2 nop nop nop decfsz loops2 goto wait2 decfsz loops goto wait ret ; generate start bit bstart bsf eeport, sdata movlw outmask tris eeport bcf eeport, sclk nop bsf eeport, sclk nop nop bcf eeport, sdata nop nop bcf eeport, sclk nop ret ; generate stop bit bstop bcf eeport, sdata movlw outmask tris eeport bcf eeport, sdata nop nop bsf eeport, sclk nop nop bsf eeport, sdata nop bcf eeport, sclk nop nop ret ; transmit byte in txbuf tx movlw 8 movwf count txlp bcf eeprom, do btfsc txbuf, 7 bsf eeprom, do movlw outmask tris eeport btfss eeprom, do goto txlp1 bsf eeport, sdata goto txlp2 txlp1 bcf eeport, sdata txlp2 bsf eeport, sclk nop nop nop bcf eeport, sclk rlf txbuf decfsz count goto txlp bsf eeprom, di movlw outmask tris eeport bsf eeport, sclk nop nop btfss eeport, sdata bcf eeprom, di bcf eeport, sclk ret ; receive byte into datai rx clrf datai movlw 8 movwf count bcf statby,0 rxlp rlf datai bsf eeprom, di movlw outmask tris eeport bsf eeport, sdata bsf eeport, sclk nop nop nop btfss eeport, sdata bcf eeprom, di bcf eeport, sclk btfsc eeprom, di bsf datai,0 decfsz count goto rxlp ret ; get byte from EEPROM into datai rbyte call bstart movlw 10100000b movwf txbuf call tx movf addr1,w ; high address movwf txbuf call tx movf addr,w ; low address snb Bflag ; if Bflag is set or w,#10000000b ; set bit 7 of low address movwf txbuf call tx call bstart movlw 10100001b movwf txbuf call tx call rx call bstop ret ; write byte in datao to EEPROM wbyte call bstart movlw 10100000b movwf txbuf call tx movf addr1,w movwf txbuf call tx movf addr,w snb Bflag ; if Bflag is set or w,#10000000b ; set bit 7 of low address movwf txbuf call tx movf datao,w movwf txbuf call tx call bstop movlw 10 movwf loops call wait ret ; BRAIN code - a self organizing self modifying (sort-of) ; learning algorthm I hacked after reading "How to build ; your own self-programming robot" by David Heiserman startup mov OPTION, #00101111b ; ext rtc,prescale wtc max (!OPTION for SPASM) mov !ra, #PortAMask ; set the A port direction bits mov !rb, #PortBMask ; and the B port bits clr BadMoves ; start counter at 0 for the wipe-core feature clrb LockNoBad ; reset the no-wipe lockout flag clr PlanBcount ; reset the plan-B counter clrb Bflag ; and the flag mov w, EnvPort ; Prime the system - get the environment and w, #EnvMask ; mask off just the input bits xor w, #ALMask ; flip the active-low bits mov ThisEnv, w ; and put into ThisEnv mov LastEnv, w ; and into LastEnv clr Action ; clear action byte mov PFCW, #InitialPFCW ; initialize program flow control word MainLoop mov addr1, #DefAddr1 ; set default address high jnb ThisLast, ml1 ; if ThisLast flag set mov w, LastEnv ; take LastEnv and w, #00011111b ; mask off top 3 bits mov addr1, w ; put in address high var ml1 mov PrevAct, Action ; save last action mov LastEnv, ThisEnv ; copy ThisEnv to LastEnv mov addr, ThisEnv ; low address for EE call rbyte ; call the read EE routine mov w, datai ; get EE data and w, #ActionMask ; filter only the response bits mov Action, w ; and save in Action mov w, datai ; get the EE data and w, #ConfMask ; filter only the confidence bits mov Confidence, w ; and save in Confidence jnz PerformAction ; if confidence > 0 then do it movb Action.MotorL1, RTCC.0 movb Action.MotorR1, RTCC.1 ; get random response movb Action.MotorL2, RTCC.2 ; from the Real Time Counter movb Action.MotorR2, RTCC.3 movb Action.MotorRev,RTCC.4 jb RMStart, ml2 ; if RMStart flag set mov Confidence, #1 ; start at confidence 0 ml2 PerformAction movb Reverse, Action.MotorRev ; reverse motor if Reverse set mov Temp, #175 ; countdown in Temp ActionLoop movb L_drive, Action.MotorL1 ;pw speed control for the motors movb R_drive, Action.MotorR1 ; 1 2 Action mov loops, #6 ; --- --- ------ call wait ; Off Off Stop movb L_drive, Action.MotorL2 ; On Off Slow movb R_drive, Action.MotorR2 ; Off On Medium mov loops, #7 ; On On Fast call wait cjne Temp, #100, al0 ; if temp < 101 jb EnvPort.L_stall, al1 ; break loop if either jb EnvPort.R_stall, al1 ; motor stalled or jb EnvPort.Feeler, al1 ; feeler touches something al0 djnz Temp, ActionLoop ; and loop until done al1 clrb L_drive clrb R_drive ; disengage motors clrb Reverse ; and reverse relay ; let's see what happened... mov !rb, #PortBMask ; set the B port bits mov w, EnvPort ; get the environment and w, #EnvMask ; mask off just the input bits xor w, #ALMask ; flip the active-low bits mov ThisEnv, w ; and put into ThisEnv mov loops, #200 call wait ; wait a bit mov w, EnvPort ; get the environment and w, #EnvMask ; mask off just the input bits xor w, #ALMask ; flip the active-low bits or ThisEnv, w ; and 'or' with ThisEnv ; determine success of move... setb Happy ; start out happy jb Bflag, eval0 ; if plan-b not in progress jnb BadMoves.ModInhib, eval0 ; if too many bad moves setb Bflag ; set the flag and mov PlanBcount, #NumPlanBs ; start the countdown eval0 mov w, PlanBcount jz eval0a ; if plan-b dec PlanBcount ; decrement the counter skip eval0a clrb Bflag ; otherwise clear the flag mov w, ThisEnv ; if ThisEnv contains sb Bflag ; (skip if plan-b) and w, #InhibA ; any inhibit-a bits snb Bflag ; (skip if plan-a) and w, #InhibB ; any plan-b inhibit bits jnz sad ; make sad jnb GetBored, eval1 ; if getbored flag set csne PrevAct, Action ; if action = last action inc Boredom ; then increment boredom count cje Boredom, #BoreFactor, sad ; if bored make sad eval1 mov w, LastEnv ; if LastEnv contains any sb Bflag ; (skip if plan-b) and w, #InhibA ; any inhibit-a bits snb Bflag ; (skip if plan-a) and w, #InhibB ; any plan-b inhibit bits jnz evalend ; then don't judge move so harshly cje Action, #Goal0, evalend cje Action, #Goal1, evalend ; happy if action cje Action, #Goal2, evalend ; meets one of the cje Action, #Goal3, evalend ; predefined 'good moves' cje Action, #Goal4, evalend cje Action, #Goal5, evalend cje Action, #Goal6, evalend sad clrb Happy ; clear the happy flag cse Boredom, #0 ; if bore count not zero dec Boredom ; decrement boredom counter evalend ; indicate state of being... (light an LED if not happy) mov !rb, #StatusMask ; set LED line to output setb StatusLED ; turn off LED sb Happy ; skip if Happy clrb StatusLED ; light LED if Sad clrb HitMax ; clear hit max flag clrb DipMax ; clear dip-from max flag jb Happy, goodmove ; jump to goodmove if Happy flag set inc BadMoves ; one more bad move jb LockNoBad, badmove ; jump to badmove if no-wipe lockout set cje BadMoves, #MaxBadMoves, eraseall ; wipe core if limit reached badmove jb LockPFCW, badm1 ; if the LockPFCW flag is not set then jnb BadMoves.ModBit,badm1 ; after so many consecutive bad moves mov loops, #250 ; wait a bit to let the call wait ; rtc bits cycle a few times mov w, RTCC ; get rtc bits and w, #ModPFMask ; mask it off xor PFCW, w ; xor PFCW with temp badm1 mov w, Confidence ; get confidence jz MainLoop ; loop if zero (confidence 0) dec Confidence ; reduce confidence by one csne Confidence, #ConfMask-1 ; skip next unless dipped from max setb DipMax w2ee mov w, Confidence ; get confidence into W or w, Action ; OR with action bits mov datao, w ; put in EE write data call wbyte ; write it (addr, addr1 already set) jnb UseGamma,Mainloop ; if UseGamma flag set jb HitMax, gamma ; call gamma routine if HitMax flag set jnb UseForget,Mainloop ; if UseForget flag set jb DipMax, gamma ; call gamma if dipmax flag set jmp MainLoop ; loop goodmove setb LockNoBad ; set the no-wipe lockout flag cjae Confidence, #ConfMask, MainLoop ; if confidence >= max just loop inc Confidence ; increment confidence jnb GMinc2, goodm1 ; if GMinc2 flag set inc Confidence ; increment it again goodm1 cjb Confidence, #ConfMask, w2ee ; if Confidence < max just write it mov Confidence, #ConfMask ; otherwise set to max setb HitMax ; set the hitmax flag jmp w2ee ; and write it to EE (along with action) ; this routine will generalize memories (hopefully) gamma jnb ThisLast, G_outer ; if ThisLast addressing mov addr1, #00011111b ; start at top of memory G_outer mov addr, #EnvMask ; sweep through each environment G_inner mov w, addr ; get low EE address and w, #EnvMask ; mask off environment xor w, #ALMask ; flip active-low bits and w, LastEnv ; any bits in common with LastEnv? jz G_next ; if not do next address call rbyte ; read EE memory jb DipMax, forget ; jump if DipMax flag set mov w, datai ; get EE data and w, #ConfMask ; filter only confidence bits jnz G_next ; already something good here - next mov w, #1 ; new confidence = 1 or w, Action ; OR with action bits mov datao, w ; put in EE write data call wbyte ; write it G_next djnz addr, G_inner ; dec addr and loop inner jnb ThisLast,MainLoop ; If ThisLast addressing djnz addr1, G_outer ; dec addr1 and loop outer jmp MainLoop forget mov Temp, datai ; get ee data and Temp, #ConfMask ; filter confidence cjne Temp, #1, G_next ; loop unless Confidence = 1 mov Temp, datai ; get ee data and Temp, #ActionMask ; check only action bits cjne Temp, Action, G_next ; loop unless Action is our action mov datao, #0 ; Set EE data to 0 call wbyte ; to kill that memory jmp G_next ; this routine solves the problem of starting core with ; leftovers of a previous run (or very bad experience) ; MaxBadMoves in a row overwrites the core with 0's ; To prevent accidental wipe from just being stuck, ; any good move at all disables this 'feature' eraseall mov !rb, #StatusMask mov datao, #0 mov addr1, #00011111b ea_l1 mov addr, #11111111b ea_l2 call wbyte djnz addr, ea_l2 xor rb, #StatusXOR ; flash when wiping core djnz addr1, ea_l1 jmp startup ;----------------------------------------------------------