* * Second version of the Huey robot, using a photocell for light * sensing. * * * Standard 68hc11 equates * iobase equ $1000 default start of I/O regs zero equ 0 dummy equate portc equ $03 i/o port c ddrc equ $07 data direction reg c oc1m equ $0c OC1 mask bits oc1d equ $0d OC1 data bits?? tcnt equ $0e timer count reg toc1 equ $16 output compare #1 toc2 equ $18 output compare #2 toc3 equ $1a output compare #3 toc4 equ $1c output compare #4 toc5 equ $1e output compare #5 tctl1 equ $20 timer control reg #1 tctl2 equ $21 timer control reg #2 tmsk1 equ $22 timer mask #1 tflg1 equ $23 timer flags reg #1 tmsk2 equ $24 timer mask #2 tflg2 equ $25 timer flags reg #2 baud equ $2b baud rate reg sccr1 equ $2c SCI reg 1 sccr2 equ $2d SCI reg 2 scsr equ $2e SCI status reg scdr equ $2f SCI data reg adctl equ $30 a/d control reg adr1 equ $31 a/d result reg 1 adr2 equ $32 a/d result reg 2 adr3 equ $33 a/d result reg 3 adr4 equ $34 a/d result reg 4 option equ $39 option reg pprog equ $3b eeprom programming reg rambeg equ $0000 start of RAM area codebeg equ $b600 start of EEPROM area stkbeg equ $00c2 top of stack area vecbeg equ $00c4 start of RAM psuedo vectors jmp.op equ $7e JMP (ext) opcode sci.vec equ vecbeg sci vector spi.vec equ vecbeg+3 spi vector paie.vec equ vecbeg+6 paie vector pao.vec equ vecbeg+9 pao vector tov.vec equ vecbeg+12 timer overflow vector toc5.vec equ vecbeg+15 toc5 vector toc4.vec equ vecbeg+18 toc4 vector toc3.vec equ vecbeg+21 toc3 vector toc2.vec equ vecbeg+24 toc2 vector toc1.vec equ vecbeg+27 toc1 vector tic3.vec equ vecbeg+30 tic3 vector tic2.vec equ vecbeg+33 tic2 vector tic1.vec equ vecbeg+36 tic1 vector rti.vec equ vecbeg+39 rti vector irq.vec equ vecbeg+42 irq vector xirq.vec equ vecbeg+45 xirq vector swi.vec equ vecbeg+48 swi vector illop.vec equ vecbeg+51 illegal opcode vector copf.vec equ vecbeg+54 cop fail vector clmf.vec equ vecbeg+57 clock monitor fail vector TRUE equ $ffff all bits set = true FALSE equ $0000 all bits clear = false rmtr.fwd equ $f880 right motor, full speed fwd rmtr.rev equ $f140 right motor, full speed rev lmtr.fwd equ $f140 left motor, full speed fwd lmtr.rev equ $f880 left motor, full speed rev rmtr.stop equ $0000 right motor stopped lmtr.stop equ $0000 left motor stopped rmtr.toc equ toc2 addr of right motor timer reg lmtr.toc equ toc3 addr of left motor timer reg spkr.toc equ toc4 addr of speaker timer reg avoiddly equ 250*3 total delay in 4-msec tics avoidp1 equ 250*1 phase 1 delay irdly1 equ 100 loop counter, startup in irtest irdly2 equ 100 loop counter, sampling time turndly equ 75 turn delay in 4-msec tics restdly equ 250*5 resting time, in 4-msec tics rundly equ 250*20 running time, in 4-msec tics redled equ %01000000 mask to turn red led on ylwled equ %00100000 mask to turn yellow led on grnled equ %00010000 mask to turn green led on offleds equ %10001111 and mask, all leds off rgtir equ %00000001 bit to turn on right LED lftir equ %00000010 bit to turn on left LED irleds equ %11111100 and-mask to turn off IR LEDs irrcvr equ %00000100 mask for ir receiver bit org rambeg start of variable declarations timers equ * start of system timer array wait rmb 2 global system timer swtimer rmb 2 timer reserved for bumper irtimer rmb 2 timer reserved for ir system pctimer rmb 2 photocell timer timersx equ * marks end of timer array swflag rmb 1 holds true/false for bumper irflag rmb 1 holds data on nearby obstacles irnow rmb 1 holds data on immediate ir scan irtemp rmb 1 temp storage for lookir routine flipflop rmb 1 run/rest flipflop ledindx rmb 1 index into light table org codebeg start of program start lds #stkbeg set top of stack ldx #iobase point x at I/O regs ldd #rmtr.stop get full stop std rmtr.toc,x and save in timer reg ldd #lmtr.stop do the same for left std lmtr.toc,x save in timer reg ldaa #$60 bits for OC2 and OC3 staa oc1m,x OC1 will control these outputs ldaa #$00 when OC1 times out... staa oc1d,x it will clear OC2 and OC3 ldaa #$f0 when OC2 and OC3 time out... staa tctl1,x they will output 1s ldd #$0000 get timeout value for OC1 std toc1,x and save in timer reg std wait zero all the timers std swtimer std irtimer std pctimer ldaa #%10000000 set a/d power up bit staa option,x and do it ldaa #%11111011 lines 7-3, 1-0 output, line 2 input staa ddrc,x make it so clr irflag show no turn in progress ldd #rti.isr setup interrupt branch to rti ISR std rti.vec+1 save in RAM area ldaa #jmp.op get a jump opcode staa rti.vec and finish up psuedo vector ldaa #%01000000 get mask for RTII staa tmsk2,x write to reg staa tflg2,x now clear any pending RTI flag cli and allow interrupts clra get all zeroes staa portc,x turn all LEDs off for now ldd #rundly get running time std pctimer start it up clr flipflop show now running clr ledindx start with index = 0 main ldd #rmtr.fwd need to go forward ldy #lmtr.fwd need to go forward jsr motors do the motors main0 ldd #1 get one count std wait and start a timer main1 ldd wait get the wait timer bne main1 wait until it hits zero jsr readad read a/d0 through a/d3 jsr testsw check the switch jsr testir test the ir object detector * jsr testpc test the photocell main1a ldaa swflag do we need to avoid? beq main2 branch if not jsr avoid do the avoid bra main0 and start again main2 ldaa irnow do we need to turn? oraa irflag or are we still turning? beq main3 branch if not jsr turn do the turn bra main0 and start again main3 * ldd pctimer need to rest? * beq main4 branch if no * jsr rest rest a bit * bra main0 and start again main4 tst flipflop running? beq main4a branch if so ldd #rmtr.stop stop both motors ldy #lmtr.stop jsr motors ldaa pctimer+1 get lsb of timer lsla divide by 2 bne main0 not 0, no need to change ldd pctimer get the timer bne main4c branch if not done ldd #rundly get running delay std pctimer save in timer clr flipflop mark as running bra main0 back to top main4c ldab ledindx get index incb and bump it cmpb #2 reached the end? ble main4b branch if not clrb yes, rewind to 0 main4b stab ledindx save for later ldy #ledtbl get table of masks aby calc addr inside table ldaa 0,y get next light jsr lights turn it on bra main0 and back to top main4a ldd pctimer get timer bne main4d branch if not done ldd #restdly get resting time std pctimer update timer inc flipflop show now resting bra main0 back to top main4d ldaa #grnled get mask for green led jsr lights turn on the led bra main do default behavior ledtbl fcb redled,grnled,ylwled table of masks * * motors -- change direction and speed of motors, based on D and Y * * X must contain base addr of I/O regs. * motors std rmtr.toc,x set the right motor sty lmtr.toc,x set the left motor rts all done * * readad -- read a/d0 through a/d3 once and leave in ADR1-ADR4 * readad ldaa #%10010000 read an0-an3 one time staa adctl,x start the conversion readad1 ldaa adctl,x get the result reg bpl readad1 loop until done rts outta here * * testsw -- check front bumper switch * testsw ldd swtimer get the switch timer beq testsw1 if 0, go check switch testsw4 ldaa #TRUE get a true flag staa swflag show switch is still closed bra testswx and leave testsw1 ldaa adr2,x get the result cmpa #$05 is a/d lower than threshold? bhi testsw3 done if not ldd #avoiddly get avoid delay std swtimer set up the timer bra testsw4 finish up testsw3 clr swflag show no switch activity testswx rts time to leave * * testpc -- check the photocell for ambient light * testpc * ldd pctimer get photocell timer * bne testpcx branch if not timed out * ldaa adr1,x get ambient light level * cmpa #$40 test against threshold * blo testpcx branch if light enough * ldd #pcdly get the resting time * std pctimer and save for later testpcx * rts and out of here * * testir -- check the IR system for approaching obstacles * testir clr irnow show nothing out there ldaa #rgtir get bits to turn on right led jsr lookir look in that direction ldaa #lftir get bits to turn on left led jsr lookir look in that direction rts all done * * lookir -- look for obstacles with requested IR LED * * Upon entry, AR holds a bit pattern that will be written to portc to * turn on an LED. This routine will then wait long enough for the ir * system to register an obstacle, if any. * * If it sees an obstacle, this routine will add one to irnow if the * right LED was on, or two if the left LED was on. This gives the * possible values for irnow: * * 0 nothing seen * 1 obstacle seen with right LED on * 2 obstacle seen with left LED on * 3 obstacle seen with right and left LEDs on * * Note that this routine checks for an obstacle with the LED on, then * turns the LED off and makes sure the object went away. False objects * are not registered in irnow. * * Upon exit, both LEDs are off. * lookir clr irtemp zero the temp variable psha save LED mask oraa portc,x or in a copy of LEDs staa portc,x turn on LED pula restore LED mask ldy #irdly1 get startup delay lookir1 dey count this time bne lookir1 loop until done ldy #irdly2 get polling delay lookir1a ldab portc,x get the i/o value andb #irrcvr leave only ir detector bit beq lookir1b branch if saw something ldaa portc,x false alarm, get current portc anda #irleds shut off IR LEDs staa portc,x and write it back bra lookir3 and leave lookir1b dey count this time bne lookir1a loop until done ldab portc,x get port value andb #irleds shut off IR LEDs stab portc,x write it back inc irtemp count it in temp variable cmpa #lftir did we see anything with left? bne lookir3 branch if not inc irtemp left eye is worth 2 counts lookir3 ldaa irtemp get temp adda irnow add to current flag value staa irnow and save it rts and leave * * avoid -- perform response to hitting bumper switch * avoid ldy #lmtr.rev left motor always backs ldd swtimer get avoid timer cpd #avoidp1 time to do phase 1? blo avoid1 branch if not ldd #rmtr.rev back up both motors bra avoidx time to leave avoid1 ldd #rmtr.fwd go forward on right motor avoidx jsr motors do the motors ldaa #redled get red pattern jsr lights turn it on rts outta here * * turn -- turn as needed to avoid an object sensed by the IR system * turn ldaa irnow get current ir results beq turn1 branch if nothing there now ldy #turndly something out there, get delay sty irtimer and reset timer tst irflag are we already turning? bne turn2 branch if yes staa irflag no, save current scan result ldaa #ylwled get yellow mask jsr lights turn it on turn2 ldaa irflag now get original scan result cmpa #01 is obstacle on right? bne turn3 branch if not turn5 ldd #rmtr.fwd move right motor foward ldy #lmtr.rev and left motor reverse jsr motors do the motors bra turnx and leave turn3 cmpa #02 is obstacle on left? bne turn4 branch if not ldd #rmtr.rev obstacle on left or ahead, ldy #lmtr.fwd and left motor forward jsr motors do the motors bra turnx and done turn4 bra turn5 object in front, always turn left turn1 ldd irtimer any time left on timer? bne turnx branch if yes clr irflag show turning is done turnx rts and outta here * * rest -- sleep until the lights come on * rest ldd #rmtr.stop stop both motors ldy #lmtr.stop jsr motors rts * * lights -- turn on selected LED * * Mask for LED to turn on is in A reg. Other LEDs will automatically * turn off. * * All registers are preserved. * lights pshb save b reg ldab portc,x get led bits andb #offleds turn all leds off stab portc,x and write it back oraa portc,x now turn on led staa portc,x and save pulb restore b reg rts * * rti.isr -- real-time interrupt service routine * rti.isr ldx #timers point x at timers rti1 ldd 0,x get next timer beq rti2 branch if already 0 subd #1 drop this timer std 0,x and save it back rti2 inx count this timer inx (timer takes 2 bytes) cpx #timersx reached the end? bne rti1 loop until done ldaa #%01000000 need to reset RTIF staa iobase+tflg2 and do it rti and outta here end