nam jerry.txt *********************************************************** * JERRY * * Jerry is a micro-processor controlled robot that will * traverse a maze starting from a point and direction specified * by the user and ending at a point also specified by the user. * This program consists of both the traversal algorithm(brain) * and the motor controller. * * The motor controller is a single thread of tasks that * run in a loop. There are three types of interruption * possible with this program. The first would be to shut off * the power to the micro-processor, the second would be to * reset the micro-processor, and the third would be to use the * control switch. If the control switch is shut off during * operation, the state of the program does not change and * Jerry can continue simply by turning on the switch again. * The program starts initially with a delay after the * control switch is turned on. The delay is about 2.5 sec. * The thread of tasks is as follows: * 1) It checks for paths at the node * 2) It translates and packages the path information * for the traversal algorithm * 3) It waits for the traversal algorithm to complete * and send back the direction to take * 4) It rotates to that direction(if not already there) * 5) It advances to the next node * This loop of events is ensured by having each task initiate * the next--along with establishing the preconditions for * that task--thereby self-destructing. * *********************************************************** * Useful declarations * PORTA equ $1000 I/O ports and control reg's PIOC equ $1002 Parallel I/O Control Reg PORTB equ $1004 output only PORTC equ $1003 unlatched Port C PORTCL equ $1005 latched Port C DDRC equ $1007 data direction reg for C PORTD equ $1008 DDRD equ $1009 PORTE equ $100A A/D inputs on 4 LSB's CFORC equ $100B Compare Force Reg OC1M equ $100C OC1 Action Mast Register OC1D equ $100D OC1 Action Data Register TCNT equ $100E Timer Counter Reg - 2 bytes TIC1 equ $1010 TIC2 equ $1012 Timer 2 Input Capture TIC3 equ $1014 TOC1 equ $1016 Timer Output Compare TOC2 equ $1018 Registers TOC3 equ $101A TCTL1 equ $1020 Timer Cntrl Reg 1 TCTL2 equ $1021 (for edges) Reg 2 TMSK1 equ $1022 Main Timer IRQ Mask R1 TFLG1 equ $1023 Main Timer IRQ Flag R1 TMSK2 equ $1024 TFLG2 equ $1025 PACTL equ $1026 Pulse Accum Cntrl Reg PACNT equ $1027 Pulse Accum Count Reg * SPCR equ $1028 SPI Control Register SPSR equ $1029 SPI Status Register SPDR equ $102A SPI Data In/Out Reg BAUD equ $102B SCI Baud "Rate" Control SCCR1 equ $102C SCI Control Register 1 SCCR2 equ $102D SCI Control Register 2 SCSR equ $102E SCI Status Register SCDR equ $102F SCI Data (Read RDR, Wr TDR) * ADCTL equ $1030 A/D control register ADR1 equ $1031 A/D reg #1 ADR2 equ $1032 ADR3 equ $1033 ADR4 equ $1034 A/D reg #4 ADR5 equ $1035 A/D reg #5 ADR6 equ $1036 A/D reg #6 ADR7 equ $1037 A/D reg #7 ADR8 equ $1038 A/D reg #8 OPTION equ $1039 System Config. Options COPRST equ $103A Arm/Reset COP Timer Circtry PPROG equ $103B EEPROM Program'g Cntrl Reg HPRIO equ $103C Highest Priority I-Bit etc. INIT equ $103D RAM - I/O mapping Reg CONFIG equ $103F COP,ROM & EEPROM enables * *********************************************************** * RAM equ 0 start of RAM STKTOP equ $004A top of stack ROM equ $C000 start of ROM <<< modified * from $E000 for Buffalo * monitor! * *********************************************************** * PrtCfg equ %00000000 Port C data direction bits * (all inputs here) * TCTL1M equ %01000000 Timer Cntrl Reg 1 Mask * |______ have Timer 2 toggle * TCTL2M equ %00001000 Timer Cntrl Reg 2 Mask * |___ Timer 2 Capture on falling * edge only * TMSK equ %01000000 Main Timer IRQ MSK Reg Mask * |______ Timer 2 Output compare IRPT * TFLGMSK equ %01000000 Mask to clear RTIF in * |______ Timer IRPT Flag Reg 2 * *********************************************************** * Program Constants TIMEr equ 14000 set for 10 ms interupts CW equ 0 Clockwise Direction CCW equ 1 Counter-clockwise Direction FWD_CNTR equ $0420 number of steps to almost get * to next node ROT_CNTR equ $0180 number of steps to almost * rotate 90 degrees UT_CNTR equ $0390 number of steps to almost * rotate 180 degrees ROT_FOR_CORR equ $20 number of steps to turn to * check paths DELAY_INIT equ $FF startup delay time * * Task Schedules Used to Relinquish/Establish Control GFI_TASK_SCH equ $81 setup for wait for Frank CPN_TASK_SCH equ $82 setup for give Frank info RAN_TASK_SCH equ $84 setup for advance to next node WFI_TASK_SCH equ $A8 setup for rotating at node ANN_TASK_SCH equ $90 setup for checking paths at node SD_TASK_SCH equ $90 setup for checking paths at node ROT_TASK_SCH equ $88 setup for an "old" rotation * * System Control Masks WAIT_ALG_MASK equ %00000001 these masks simply SU_ALG_MASK equ %00000010 represent the bits ADVANCING_MASK equ %00000100 of the CONTROL byte ROTATING_MASK equ %00001000 CHECKING_MASK equ %00010000 NEW_ROT_MASK equ %00100000 STARTUP_MASK equ %01000000 CONT_SW_MASK equ %10000000 * * Sensor Decoding Masks PER_SEN_MASK equ %00111111 these masks CENT_SEN_MASK equ %00000111 correspond to the CENTER_SENSOR equ %00000100 hardware inputs FRONT_SENSOR equ %00000001 to PORTC or PORTE REAR_SENSOR equ %00000010 LEFT_SENSOR equ %00010000 RIGHT_SENSOR equ %00100000 * * Direction Masks SOUTH_DIR equ %00000001 these masks WEST_DIR equ %00000010 correspond to the NORTH_DIR equ %00000100 directions used in the EAST_DIR equ %00001000 traversal algorithm * LKAVAIL equ $DD00 list of links available * LKTAKEN equ $DD36 list of links traversed * ALGTOP equ $DDFF top of algorithm stack * OUTSTRG equ $FFC7 INCHAR equ $FFCD INPUT equ $FFAC OUTCRLF equ $FFC4 UPCASE equ $FFA0 SETBTCKMSK equ %00000001 SETNXTNDEMSK equ %00000010 SETDONEMSK equ %10000000 SETRCHKMSK equ %00000100 STARTUPMSK equ %00001000 * *********************************************************** * Task Control Block for motors * motStepDir equ 0 Motor Step Direction motOutRefNum equ 1 Motor Output Reference Number motOutComm equ 2 Motor Output Command(bit pattern) motOutMask equ 3 Motor Output Mask * *********************************************************** * Reserved RAM org RAM CONTROL rmb 1 Physical system control byte * 0 -> wait for Frank * 1 -> setup for Frank * 2 -> advancing to next node * 3 -> rotating at node * 4 -> checking paths at node * 5 -> new rotation * 6 -> startup delay * 7 -> system control switch CURR_DIR rmb 1 the current direction that * Jerry is facing NEXT_DIR rmb 1 the next direction that * Jerry will be facing PERIMETERS rmb 1 last reading of the * perimeter sensors CENTERS rmb 1 last reading of the * center sensors MARK rmb 1 gives Frank the info he * needs to traverse: * MSB -> control * 4 LSB's -> available paths FRANK rmb 1 gives Mark the info he * needs to traverse: * MSB -> control * 4 LSB's -> direction to go DELAY rmb 1 startup delay timer LMTCB rmb 4 left motor task control block RMTCB rmb 4 right motor task control block SCRATCH1 rmb 1 scratch variable SCRATCH2 rmb 1 scratch variable SCRATCH3 rmb 1 scratch variable SCRATCH4 rmb 2 scratch variable ND2INDX rmb 1 node to be index into * CNTRLFRNK rmb 1 CNODE rmb 1 Current node in maze * PRVNDE rmb 1 previous node * NXTNDE rmb 1 Next Node to be visited * FNODE rmb 1 Destination node in maze * YCOORD rmb 1 Y coordinate entered by user * XCOORD rmb 1 X coordinate entered by user * ALGSTK rmb 2 TBLSTK rmb 2 * *********************************************************** * Buffalo Monitor interrupt jump vectors * org $00dc Timer 2 Output Compare jmp timer2 * *********************************************************** * System Initializations * org ROM start sei set interrupt mask ldaa #$d0 disable stop, X-interpt tap etc. disable everything lds #STKTOP initialize stack pointer * * RAM initialization * ldab #33 number of bytes used for variables ldx #CONTROL load the address of CONTROL ldy #RT load the address of the RAM template loop ldaa 0,y loop inits RAM with the RAM template sta 0,x from RAM 0 to 33 inx iny decb bne loop * * Test Maze Initialization * * load the starting address of the TEST maze in X ldx #TEST * store the starting address of the TEST maze in TC stx TC * * I/O Device Initialization * ldaa #PrtCfg set up direction of port C staa DDRC input bits * jsr getInfo get the user information ldx TBLSTK load pointer to rom tables ldd #$100 load counter into A jsr initROM initialize ROM memory to zero * * Counter and Interrupt Initialization * ldd TCNT get current "time" (counts) addd #TIMEr set up counter #2 to * generate its first irupt at * present time + TIMEr std TOC2 Cntr 2 Output Compare Reg ldaa #TMSK enable timer 2 output staa TMSK1 compare interrupts cli enable interrupts * * * fall through to a dumb background job * backgnd nop bra backgnd * *********************************************************** * Timer 2 interrupt handling routine * * This routine reschedules itself to interrupt * again by (a) assuming that the present time * (count value) in TCNT is approximately that * in "nextcnt", (b) adding * the value COUNTV to the "nextcnt", and * (c) storing that sum in the timer 2 output * compare register TOC2 and back in "nextcnt". * timer2 ldab #TFLGMSK clear OC2F bit (flag) stab TFLG1 in TFLG1 reg to clear IRPT * ldd TOC2 get current "time" (counts) addd #TIMEr set "time" of next counter2 std TOC2 output compare interrupt * *********************************************************** * REALTIME TASK SCHEDULER *********************************************************** * Purpose: * To assign tasks based on the system CONTROL byte setting. * The CONTROL byte has exclusive control over issuing tasks. * A task will "self-destruct" after it reaches it's goal * by setting CONTROL to the next task and establishing * the preconditions for that task. * * Preconditions: None * * Memory Accessed: CONTROL * * Registers Over-Written: None * * Register Values Left: N/A * * Subroutines Called: * Read/Store Sensors Values(RSSV) * Startup Delay(SD) * Wait for Frank's Information(WFI) * Give Frank Information(GFI) * Advance to Next Node(ANN) * Rotate at Node(RAN) * Checks for Paths at Node(CPN) * * branch on CONTROL byte information to the * appropriate task jsr RSSV brclr CONTROL CONT_SW_MASK GT_STARTUP brset CONTROL WAIT_ALG_MASK GT_WAITALG brset CONTROL SU_ALG_MASK GT_SUFRANK brset CONTROL ADVANCING_MASK GT_ADVANCE brset CONTROL ROTATING_MASK GT_ROTATE brset CONTROL CHECKING_MASK GT_CHECK brset CONTROL STARTUP_MASK GT_STARTUP * If an error occurs, then the program will sit in a * tight loop and not release the interrupt so that * nothing in memory will change before it can be * examined. An error occurs when the control switch * is set but no tasks were ordered. ERROR bra ERROR GT_STARTUP jsr SD bra LEAVE_INT * * NOTE: jsr to Frank's algorithm first, then to WFI * GT_WAITALG jsr ALGRTHM jsr WFI bra LEAVE_INT GT_SUFRANK jsr GFI bra LEAVE_INT GT_ADVANCE jsr ANN bra LEAVE_INT GT_ROTATE jsr RAN bra LEAVE_INT GT_CHECK jsr CPN LEAVE_INT rti * *********************************************************** ********************************************************************** * Name: ANd2Tbl * Purpose: Adds the current node to the algorithm stack. This * function is only used when the we are not in LOOP mode * Memory Accessed: * CNODE * Registers Over-Written: * B <- CNODE * Register Values Left: * B contains cnode * Subroutines Called: * pshBX ANd2Tbl ldab CNODE load the current node ldx ALGSTK load the algorithm stack address stab 0,x dex stx ALGSTK store the new algorithm stack address rts ********************************************************************** ************************************************************************* * Name: Algorithm * Tag: ALGRTHM * * Purpose: * This is the main control algorithm for the brain. It * calls the appropriate routines to determine what Jerry * should do next. * * Preconditions: * None * Memory Accessed: * CNTRLFRNK * Registers Over-Written: * A B * Register Values Left: * A <- CNTRLFRNK * B <- signal for ending program * Subroutines Called: * upd_nt CHKEND CHKLP CHKFDE * CHKRDE PRNMAZE BTRACK NXTNDE * SETUPCNTRL ************************************************************************* ALGRTHM ldaa MARK cmpa #$80 beq wereDone ldaa CNTRLFRNK load the control byte cmpa #SETDONEMSK see if were done bne rchkLoop were not done, continue wereDone ldab #$80 get ready to set marks byte stab FRANK let mark know were done bra ALGRTHM_end were done, end the routine rchkLoop jsr upd_nt jsr CHKLP see if were in a loop jsr CHKRDE see if were at a real dead end jsr CHKFDE see if were at a fake dead end jsr PRNMAZE see if we need to prune stack ldaa CNTRLFRNK load the control byte cmpa #SETRCHKMSK see if we need to recheck bne startBtrack recheck not set, goto btrack ldaa #SETNXTNDEMSK load the next node mask staa CNTRLFRNK and nxtnde in CNTRLFRNK bra rchkLoop we need to recheck startBtrack jsr BTRACK try to backtrack jsr FINDNXTNDE try to go to the next node jsr SETUPCNTRL setup for next time through ALGRTHM_end rts return to calling routine ********************************************************************* * Name: BackTrack * Tag: BTRACK * * Purpose: Checks to see if the backtrack bit is set in CNTRLFRNK. * If it isn't, exit the routine. Otherwise, pop the next * node from the stack. See which direction is node is in * compared to the current node and then set frank's byte to * go there. * * Preconditions: * We have either detected a loop or either of the dead-ends * Memory Accessed: * CNTRLFRNK CNODE ALGSTK * Registers Over-Written: * A B X * Register Values Left: * A <- the current node * B <- node from the stack * X <- stack pointer * Subroutines Called: * None ********************************************************************** BTRACK ldaa CNTRLFRNK load the control byte cmpa #SETBTCKMSK see if backtrack is set bne BTRACK_end btrack not set...exit ldx ALGSTK load the stack pointer * cmpx #ALGTOP * beq mazeDone ldaa CNODE load current node inx increment the stack pointer ldab 0,x pop node from the stack stx ALGSTK update the stack pointer sba subtract node numbers cmpa #$FF was previous node east bne wasNrth if not, was it north? jsr moveE move East bra BTRACK_end end routine wasNrth cmpa #$FA was previous node north bne wasWest if not, was it west? jsr moveN move North bra BTRACK_end end routine wasWest cmpa #$1 was previous node west bne wasSth if not, was it south? jsr moveW move West bra BTRACK_end end routine wasSth jsr moveS otherwise it must be south bra BTRACK_end end routine *mazeDone * ldaa #SETDONEMSK were done with the maze * staa CNTRLFRNK store this to cntrl BTRACK_end rts return to calling routine ********************************************************************* * Name: Find Next Node * Tag: FINDNXTNDE * * Purpose: Checks to see if the Next Node bit (bit 1) is set in * CNTRLFRNK. If it isn't, then the routine is exited. * Otherwise, the next node to be visited is determined and * the correct pattern is assigned in FRANK. The algorithm * gives priority to unvisited nodes. If all nodes are * unvisited priority is assigned in the following order. * * 1 - East * 2 - North * 3 - West * 4 - South * * Preconditions: * None * Memory Accessed: * CNODE CNTRLFRNK ND2INDX SCRATCH1 * Registers Over-Written: * A B X Y * Register Values Left: * A <- direction bit to take * B <- CNODE * X <- links available table * Y <- links traversed table * Subroutines Called: * index moveE moveN moveW moveS ********************************************************************* FINDNXTNDE ldaa CNTRLFRNK load the control byte cmpa #SETNXTNDEMSK see if next node bit is set bne FINDNXTNDE_end if not, exit the routine ldaa CNODE load the current node staa ND2INDX get ready to index this node ldx #LKAVAIL load the links available table ldy #LKTAKEN load the links travered table jsr index calculate this nodes offset abx add offset to available table aby add offset to traversed table ldaa 0,x load available links at node eora 0,y see which links can be traversed ldab CNODE load the current node andb #$1 mask everything except bit 0 beq maskE node is even anda #$F0 mask off even stuff lsra move data to the lsb's lsra lsra lsra bra chkEast start checking directions maskE anda #$F ask off odd stuff chkEast bita #$8 see if its a east move beq chkNrth not east move, check north ldaa #$88 load east move bit staa SCRATCH1 and store it away jsr moveE set FRANK to east move bra setDir update table chkNrth bita #$4 see if its a west move beq chkWest not north move, check west ldaa #$44 load north move bit staa SCRATCH1 and store it away jsr moveN set FRANK to north move bra setDir update table chkWest bita #$2 see if its a west move beq chkSth not west move, check south ldaa #$22 load west move bit staa SCRATCH1 and store it away jsr moveW set FRANK to west move bra setDir update table chkSth ldaa #$11 load south bit staa SCRATCH1 and store it away jsr moveS set FRANK to south move setDir jsr ANd2Tbl ldaa CNODE load the current node anda #$1 mask off bits 1-7 beq evenNd branch if its a even node ldaa SCRATCH1 load movement bit anda #$F0 mask off lsb's oraa 0,y mark table that this link staa 0,y has been traversed bra FINDNXTNDE_end branch to exit the routine evenNd ldaa SCRATCH1 load movement bits anda #$F mask off msb's oraa 0,y mark table that this link staa 0,y has been traveresed FINDNXTNDE_end rts return to calling routine ********************************************************************* * Name: Setup Control For Next Time * Tag: SETUPCNTRL * * Purpose: * Sets the contrl byte (CNTRLFRNK) for the nextime we enter * brain routine. * * Preconditions: * None * * Memory Accessed: * CNTRLFRNK * Registers Over-Written: * A B * Register Values Left: * A <- CNTRLFRNK * B <- NXTNODE * Subroutines Called: * None ********************************************************************** SETUPCNTRL ldaa CNODE load the current node staa PRVNDE store it as the previous node ldab NXTNDE get the next node and store this stab CNODE to the current node cmpb FNODE see if were at the final node beq setDONE if so, set done to true ldaa #SETNXTNDEMSK load the next node mask staa CNTRLFRNK store this to the control byte bra SETUPCNTRL_end end the routine setDONE ldaa #SETDONEMSK set the done bit (bit 7) in staa CNTRLFRNK CNTRLFRNK SETUPCNTRL_end rts return to calling routine ************************************************************************* * Name: Prune Maze * Tag: PRNMAZE * * Purpose: Checks to see if the backtrack bit (bit 0) is set in * CNTRLFRNK. If it isn't, then exit the routine. Otherwise, * check if there a loop has occured. If on has check to see * if there are any available paths from the top of the stack * to where current node appears in the stack. If there is then * exit the routine. If there isn't then set the stack pointer * to where current node appears in the stack and take off from * there rechecking for dead-ends and loops. Otherwise, exit * the routine and move to the next node. * * Preconditions: * None * Memory Accessed: * CNODE ALGSTK SCRATCH1 CNTRLFRNK * Registers Over-Written: * A B X Y * Register Values Left: * A <- CNTRLFRNK * CNODE * Table Information * B <- Node Number * Node index * X <- LKAVAIL * LKTAKEN * Y <- Stack Pointer * Subroutines Called: * index ************************************************************************* PRNMAZE ldaa CNTRLFRNK load the control byte cmpa #SETBTCKMSK see if backtrack is set bne PRNMAZE_end if not, exit the routine ldy ALGSTK load the algorithm stack pointer contPrn cmpy #ALGTOP are we at the top of the stack beq mazeDone iny increment stack pointer ldab 0,y get top node on stack ldaa CNODE load the current node cba compare the two nodes beq resetPtr if so, reset the pointer stab ND2INDX store the current node to index it ldx #LKAVAIL load the links available table jsr index calculate current node index abx add node offset to table ldaa 0,x get available links information ldx #LKTAKEN get links traversed information abx add node offset to table eora 0,x see if there any paths left ldab ND2INDX load the current node andb #$1 and see if its even or odd bne prnOdd if odd, branch to mask even off anda #$F its even, mask odd stuff off beq contPrn no paths at this bra PRNMAZE_end found a path, exit prnOdd anda #$F0 mask off even bits beq contPrn if not, check next node bra PRNMAZE_end available paths detected, exit mazeDone ldaa #SETDONEMSK staa CNTRLFRNK resetPtr ldaa #SETRCHKMSK set recheck (bit 2) in staa CNTRLFRNK in the control byte sty ALGSTK update stack pointer PRNMAZE_end rts return to the calling routine ************************************************************************ * Name: Check For Loop * Tag: CHK4LP * * Purpose: Checks to see if the nxtnde bit (1) in CNTRLFRNK is * set and exits if it isn't. Otherwise, search the * contents of the stack to see if the current node * (CNODE) is already contained in it. If it is then * unset the nxtnde bit and set the backtrack bit (0). * If CNODE doesn't already appear in the stack then * exit this routine, without modifying CNTRLFRNK. * * Preconditions: * * Memory Accessed: * CNTRLFRNK CNODE ALGSTK * Registers Over-Written: * A B X * Register Values Left: * A <- #$1 * B <- Node Number popped from stack * X <- Stack pointer position * Subroutines Called: * None ************************************************************************ CHKLP ldaa CNTRLFRNK load the control byte cmpa #SETNXTNDEMSK see if nxtnde bit is set bne CHKLP_end nxtnde not set...exit ldx ALGSTK load the algorithm stack pointer contcklp ldaa CNODE load the current node cmpx #ALGTOP see if were at the top of stack beq CHKLP_end if so end, not setting btrack inx increment the stack pointer ldab 0,x otherwise load next node on stack cba compare the nodes beq loopDet if there equal, we found a loop bra contcklp check the next node on the stack loopDet ldaa #SETBTCKMSK set the backtrack bit in CNTRLFRNK staa CNTRLFRNK b/c we found a loop CHKLP_end rts return to calling routine ********************************************************************* * Name: Check For Real Dead End * Tag: CHKRDE * * Purpose: * If the nxtnde bit is set (bit 1), the subroutine is exited. * Otherwise, we check to see if we have encountered a "real" * dead-end. A real dead is detected when there is only one * link into the node. This is handled by backing back out of * the node. * . * | * A . -- . **Three real-dead ends off** * | **of node A. ** * . * * Preconditions: * None * Memory Accessed: * MARK CNTRLFRNK * Registers Over-Written: * A * Register Values Left: * A <- current nodal information * Subroutines Called: * None ********************************************************************** CHKRDE ldaa CNTRLFRNK load the control byte cmpa #STARTUPMSK see if were on startup beq setNxtNdeSt if so, set nxtnde ldaa CNTRLFRNK load the control byte cmpa #SETNXTNDEMSK see if nxtnde is set bne CHKRDE_end nxtnde isn't set so leave ldaa MARK load current nodal information anda #$F mask off the control bit cmpa #$1 see if we only have a south path beq RDE_det real dead-end detected cmpa #$2 see if we only have a west path beq RDE_det real dead-end detected cmpa #$4 see if we only have a north path beq RDE_det real dead-end detected cmpa #$8 see if we only have a east path beq RDE_det real dead-end detected bra CHKRDE_end setNxtNdeSt ldaa #SETNXTNDEMSK load the next node mask staa CNTRLFRNK store it to CNTRLFRNK bra CHKRDE_end end the routine RDE_det ldaa #SETBTCKMSK set backtrack bit in CNTRLFRNK, staa CNTRLFRNK b/c we found a real dead end CHKRDE_end rts ********************************************************************* * Name: Check For Fake Dead End * Tag: CHKFDE * * Purpose: Checks it see if the nxtnde bit (bit 1) is set in CNTRLFRNK. * If it isn't the routine is exited. Otherwise, we check if * we are at a fake dead-end. A fake dead end occurs when we * are at a node and we have already visted all of the paths * out of that node. * * Preconditions: * None * Memory Accessed: * CNODE ND2INDX CNTRLFRNK * Registers Over-Written: * A B X Y * Register Values Left: * A <- untaken paths at a node * B <- the nodes index into the table * X <- pointer to links available table * Y <- pointer to links traversed table * Subroutines Called: * index ********************************************************************** CHKFDE ldaa CNTRLFRNK load the control byte cmpa #SETNXTNDEMSK see if nxtnde bit is set bne CHKFDE_end nxtnde not set...exit ldx #LKAVAIL load the links available table ldy #LKTAKEN load the links traversed table ldaa CNODE load the current node staa ND2INDX get ready to index current node jsr index calculate current nodes offset abx add offset into LKAVAIL table aby add offset into LKTAKEN table ldaa 0,x get available links at CNODE ldab CNODE load the current node to andb #$1 see if its odd or even bne MskOdd if odd, branch anda #$F its even, mask off odd stuff ldab 0,y load the traversed table andb #$F mask off odd stuff stab SCRATCH1 store values bra eorData go to exor the data MskOdd anda #$F0 mask off the even stuff ldab 0,y load the traversed table andb #$F0 mask off the even stuff stab SCRATCH1 store values eorData eora SCRATCH1 find untaken links at CNODE bne CHKFDE_end not at a fake dead end ldab #SETBTCKMSK load backtrack mask and set stab CNTRLFRNK its bit in CNTRLFRNK CHKFDE_end rts ********************************************************************** * Name: upd_nt * Purpose: This function updates the node table with the * information containing the avaialable node paths. * Also sets the path into the node as being traversed * by the next node so we can't go back over it. * Memory Accessed: * CNODE NXTNDE PRVNDE MARK SCRATCH2 * Registers Over_Written: * A * Register Values Left: * A has ordered node data * Subroutines Called: index * upd_nt ldx #LKTAKEN Load links traversed table ldaa CNODE staa ND2INDX set the node to be indexed ldab PRVNDE load the previous node sba See which direction the prev. node was cmpa #$FF Came in from the east beq inEst cmpa #$FA Came in from the north beq inNrth cmpa #$1 Came in from the West beq inWst cmpa #$6 Came in from the south beq inSth cmpa #$0 at the same node are you were previsouly jsr udp_Avl bra upd_end * inEst ldaa #$80 These values set register A so that jsr upd_Tkn the links traversed table can be bra upd_end inNrth ldaa #$40 set to the proper value. jsr upd_Tkn bra upd_end inWst ldaa #$20 jsr upd_Tkn bra upd_end inSth ldaa #$10 jsr upd_Tkn bra upd_end upd_end rts ********************************************************************** ********************************************************************** * Name: upd_Tkn, udp_Avl * Purpose: This function updates the node table with the * information containing the avaialable node paths. * Also sets the path into the node as being traversed * by the next node so we can't go back over it. * Memory Accessed: * CNODE NXTNDE PRVNDE MARK SCRATCH2 * Registers Over_Written: * A * Register Values Left: * A has ordered node data * Subroutines Called: index * upd_Tkn staa SCRATCH2 Update the contents of the links jsr index traversed table by indexing into it abx and OR the link coming into the node oraa 0,x staa 0,x udp_Avl ldx #LKAVAIL load table containing information * about the links at a node ldaa MARK load the new nodal information and anda #$F get it read to put it into table lsla left shift the data four times lsla because the lower byte contains lsla the even information lsla staa SCRATCH2 jsr index calculates the index into the table abx add the index to the starting location oraa 0,x or stored information stored w/new staa 0,x and store it back in bclr MARK $80 rts moveE ldaa CNODE load current node staa PRVNDE update previous node with the current inca increment the current node number staa NXTNDE update the next node with this ldaa #$88 set east direction bit staa FRANK rts moveN ldaa CNODE load current node staa PRVNDE update previous node with the current ldab #$6 calculate the next node number aba staa NXTNDE update the next node with this ldaa #$84 set north direction bit staa FRANK rts moveW ldaa CNODE load the current node staa PRVNDE update previous node with the current deca decrement the current node number staa NXTNDE update the next node number ldaa #$82 set west direction bit staa FRANK rts moveS ldaa CNODE load the current node staa PRVNDE update previous node with the curent ldab #$6 calculate the next node number sba staa NXTNDE update the next node with this ldaa #$81 set south direction bit staa FRANK rts ********************************************************************** * Name: index * Purpose: Calculates the index into the table. Since the * byte contains two nodes in it, it will need some * calculations. The odd numbered nodes are the MSB's * and the even number nodes are the LSB's. The last * bit is checked to see if the node is even or odd * and the appropriate action is done to it * Memory Accessed: * ND2INDX * Registers Over-Written: * None * Register Values Left: * A has ordered node data * B has offset into table * Subroutines Called: * even odd ********************************************************************** index ldab ND2INDX load the node number to be indexed tba anda #$1 mask off everything except bit 0 beq even The node is an even number odd decb decrement register b and divide it lsrb by two to get the correct entry into * the table ldaa SCRATCH2 load the new node information bra indexD even lsrb right shift the node to divide it * by two to get the correct entry into * the table ldaa SCRATCH2 load the new node information lsra since even nodes are the lsb's lsra the information needs to be shifted lsra done to the lsb's lsra indexD rts indexing is done ********************************************************************** * Name: getInfo * Purpose: Gets the information from the user. The user is prompted * for the starting and finishing nodes. They are then * asked to confirm their choices, or reenter them. * Memory Accessed: * XCOORD YCOORD CNODE FNODE * Registers Over-Written: * Register Values Left: * Subroutines Called: * None ********************************************************************** getInfo ldx #MSG0 load the message 0 location jsr OUTSTRG send message 0 to the screen ldx #MSG01 load the message 01 location jsr OUTSTRG send the message 01 to the screen ldx #MSG1 load message 1 location jsr SMess staa XCOORD store it as the X-coordinate ldx #MSG2 load message 2 location jsr SMess staa YCOORD store it as the Y-coordinate jsr usr2prg convert X,Y to a node staa CNODE store it as the current node staa PRVNDE staa NXTNDE ldx #MSG2B load message 2B location into x jsr OUTSTRG send it out to the screen jsr getchar read the character jsr UPCASE jsr StrtDir jsr OUTCRLF output carriage return ldx #MSG3 load message 3 location jsr SMess staa XCOORD store it as the X-coordinate ldx #MSG4 load message 4 location jsr SMess staa YCOORD store it as the Y-coordinate jsr usr2prg convert X,Y to a node staa FNODE store it as the final node jsr OUTCRLF output carriage return ldx #MSG5 load message 5 location jsr OUTSTRG send message 5 to the screen jsr getchar get the character from the user jsr UPCASE convert it to upper case suba #$59 see if its a yes or no bne getInfo if its a no get the data again jsr OUTCRLF send a carriage return to screen ldx #MSG6 load message 6 location jsr OUTSTRG send message 6 to the screen ldx #MSG7 load message 7 location jsr OUTSTRG send message 7 to the screen rts ********************************************************************** * Name: SMess * Purpose: * Memory Accessed: * Registers Over-Written: * Register Values Left: * Subroutines Called: OUTSTRG, getchar * SMess jsr OUTSTRG send message 1 to the screen jsr getchar get the character from user anda #$0F convert it to integer cmpa #$0 make sure it is between 0 and 5 blo SMess cmpa #$5 bhi SMess rts ********************************************************************** *************************************************************************** * Name: getchar * Purpose: Gets the character the user entered from the keyboard. The * INPUT routine provided by the EVB is used. The routine waits * for the user to enter data. This data is then stored to the * A accumulator. * Memory Accessed: * None * Registers Over-Written: * Register Values Left: * Subroutines Called: INCHAR * getchar jsr INCHAR get character from keyboard tsta see if we got anything beq getchar if not, try again rts *************************************************************************** *********************************************************************** * Name: usr2prg * Purpose: Takes information in the form X,Y and calculates a node * number based on this information * Memory Accessed: * YCOORD XCOORD * Registers Over-Written: * A * Register Values Left: * A <- YCOORD*6 + XCOORD * Subroutines Called: * usr2prg ldaa YCOORD load the Y-coordinate into A lsla multiply it by four lsla adda YCOORD add it to itself twice to make it adda YCOORD multiplied by 6 and add the adda XCOORD X-Coord to it. rts *********************************************************************** *********************************************************************** * Name: StrtDir * Purpose: Given the character in the A accumulator the corresponding * direction is assigned as the current direction. This * function is only called on startup when the user enters * the starting direction. * Memory Accessed: * CURR_DIR * Registers Over-Written: * None * Register Values Left: * A <- CURR_DIR * Subroutines Called: * StrtDir clr CURR_DIR cmpa #$45 See if we are starting eastward beq StrtEst cmpa #$4e See if we are starting north beq StrtNrh cmpa #$57 See if we are starting West beq StrtWst cmpa #$53 See if we are starting South beq StrtSth StrtEst bset CURR_DIR $8 Set current direction to east bra DnSetDr StrtNrh bset CURR_DIR $4 Set current direction to north bra DnSetDr StrtWst bset CURR_DIR $2 Set current direction to west bra DnSetDr StrtSth bset CURR_DIR $1 Set current direction to south DnSetDr rts *********************************************************************** ********************************************************************** * Name: initROM * Purpose: Initializes the rom locations used for table data and * stacks to 0. Since ROM ($C000-$DFFF) is not initialized to * 0 it must be done here. * Memory Accessed: * None * Registers Over-Written: * A holds coutner information * Register Values Left: * A <- $00 * Subroutines Called: * initROM clr 0,x clear the data at x inx increment the x pointer subd #$1 decrement the counter bne initROM continue if not done rts *********************************************************** * SUBROUTINE *********************************************************** * Name: Test Routine * Tag: TR * * Purpose: * To load FRANK's byte with a hard-coded maze in ROM to * test hardware performance. * * Preconditions: None * * Memory Accessed: CONTROL, FRANK, TC * * Registers Over-Written: A, B, X * * Register Values Left: * A -> the TEST maze data * B -> end of maze error condition when maze is finished * X -> the address of the TEST maze data * * Subroutines Called: None * TR ldx TC ldaa 0,x beq TR_S staa FRANK inx stx TC bra TR_END TR_S ldab $80 stab CONTROL bra TR_END TR_END rts * *********************************************************** *********************************************************** * SUBROUTINE *********************************************************** * Name: Rotate at Node * Tag: RAN * * Purpose: * Interprets the information from the traversal algorithm * and translates it to hardware commands. * * Preconditions: None * * Memory Accessed: NEXT_DIR, CURR_DIR, SCRATCH4, CONTROL * * Registers Over-Written: A, D(A & B) * * Register Values Left: * A -> the updated CONTROL byte(RLAN_TASK_SCH) * B -> junk * * Subroutines Called: * Rotate Right at Node(RRAN) * Rotate Left at Node(RLAN) * Rotate 180 Degrees at Node(UTURN) * RAN ldaa NEXT_DIR load the next direction in A * if the current direction is south, * then use that as a reference brset CURR_DIR SOUTH_DIR SRAN * if the current direction is west, * then use that as a reference brset CURR_DIR WEST_DIR WRAN * if the current direction is north, * then use that as a reference brset CURR_DIR NORTH_DIR NRAN * if the current direction is east, * then use that as a reference brset CURR_DIR EAST_DIR ERAN SRAN bita #SOUTH_DIR see if SOUTH_DIR is set beq SRAN_1 branch if not SOUTH_DIR jmp NO_ROT branch always SRAN_1 bita #WEST_DIR see if WEST_DIR is set beq SRAN_2 branch if not WEST_DIR bra ROT_R branch always SRAN_2 bita #NORTH_DIR see if NORTH_DIR is set beq SRAN_3 branch if not NORTH_DIR bra ROT_180 branch always SRAN_3 bra ROT_L branch always WRAN bita #SOUTH_DIR see if SOUTH_DIR is set beq WRAN_1 branch if not SOUTH_DIR bra ROT_L branch always WRAN_1 bita #WEST_DIR see if WEST_DIR is set beq WRAN_2 branch if not WEST_DIR bra NO_ROT branch always WRAN_2 bita #NORTH_DIR see if NORTH_DIR is set beq WRAN_3 branch if not NORTH_DIR bra ROT_R branch always WRAN_3 bra ROT_180 branch always NRAN bita #SOUTH_DIR see if SOUTH_DIR is set beq NRAN_1 branch if not SOUTH_DIR bra ROT_180 branch always NRAN_1 bita #WEST_DIR see if WEST_DIR is set beq NRAN_2 branch if not WEST_DIR bra ROT_L branch always NRAN_2 bita #NORTH_DIR see if NORTH_DIR is set beq NRAN_3 branch if not NORTH_DIR bra NO_ROT branch always NRAN_3 bra ROT_R branch always ERAN bita #SOUTH_DIR see if SOUTH_DIR is set beq ERAN_1 branch if not SOUTH_DIR bra ROT_R branch always ERAN_1 bita #WEST_DIR see if WEST_DIR is set beq ERAN_2 branch if not WEST_DIR bra ROT_180 branch always ERAN_2 bita #NORTH_DIR see if NORTH_DIR is set beq ERAN_3 branch if not NORTH_DIR bra ROT_L branch always ERAN_3 bra NO_ROT branch always * if not the start of a new task, then * don't set preconditions ROT_R brclr CONTROL NEW_ROT_MASK ROT_R1 ldd #ROT_CNTR preconditions for RRAN std SCRATCH4 store D in SCRATCH4 ldaa #ROT_TASK_SCH set to "old" task staa CONTROL update the CONTROL byte ROT_R1 jsr RRAN subroutine to RRAN bra RAN_END branch always * if not the start of a new task, then * don't set preconditions ROT_L brclr CONTROL NEW_ROT_MASK ROT_L1 ldd #ROT_CNTR preconditions for RLAN std SCRATCH4 store D in SCRATCH4 ldaa #ROT_TASK_SCH set to "old" task staa CONTROL update the CONTROL byte ROT_L1 jsr RLAN subroutine to RLAN bra RAN_END branch always * if not the start of a new task, then * don't set preconditions ROT_180 brclr CONTROL NEW_ROT_MASK ROT_U1 ldd #UT_CNTR preconditions for UTURN std SCRATCH4 store D in SCRATCH4 ldaa #ROT_TASK_SCH set to "old" task staa CONTROL update the CONTROL byte ROT_U1 jsr UTURN subroutine to UTURN bra RAN_END branch always NO_ROT ldd #FWD_CNTR set preconditions std SCRATCH4 ldaa #RAN_TASK_SCH give control to ANN staa CONTROL update the CONTROL byte RAN_END rts * *********************************************************** *********************************************************** * SUBROUTINE *********************************************************** * Name: Startup Delay * Tag: SD * * Purpose: * Check the system control switch--if the switch is off, * then don't do anything. If the switch is on, then * run a delay loop before handing off control to the * next task. Before going to the next task, re-initialize * the delay and setup for that task. * * Preconditions: * 1) store $FF in DELAY * * Memory Accessed: CONTROL, DELAY, SCRATCH1, SCRATCH3 * * Registers Over-Written: A * * Register Values Left: * A -> updated CONTROL byte * * Subroutines Called: * Read/Store Sensor Values(RSSV) * SD jsr RSSV check the control switch * if the control switch is off then don't do anything brclr CONTROL CONT_SW_MASK SD_END ldaa DELAY load the delay value in A deca decrement A beq SD_1 if delay is done then move on staa DELAY store the new delay bra SD_END branch always SD_1 ldaa #DELAY_INIT load delay constant in A staa DELAY reinitialize the delay ldaa #1 satisfy preconditions staa SCRATCH1 for Check Paths at Node ldaa #ROT_FOR_CORR staa SCRATCH2 clr SCRATCH3 ldaa #SD_TASK_SCH give control to CPN staa CONTROL update the CONTROL byte SD_END rts * *********************************************************** *********************************************************** * SUBROUTINE *********************************************************** * Name: Wait for Franks Information * Tag: WFI * * Purpose: * Wait for Frank to determine the next direction and * give back control of the processor. *****A hard-coded test maze can be used if the following *****lines is placed as the first line in this subroutine: ***** jsr TR *****NOTE: don't forget to pre-initialize a current direction * * * Preconditions: None * * Memory Accessed: FRANK, CONTROL * * Registers Over-Written: A * * Register Values Left: * A -> updated CONTROL byte * * Subroutines Called: None * * check the control bit to see if Frank is done WFI brclr FRANK CONT_SW_MASK WFI_END * clear the control bit bclr FRANK CONT_SW_MASK ldaa FRANK load Franks info in A bne WFI_1 branch if no error clr CONTROL clear CONTROL byte if error bset #CONTROL CONT_SW_MASK set bit 7 bra WFI_END branch always WFI_1 staa NEXT_DIR store Franks info as the * next direction ldaa #WFI_TASK_SCH give control to RAN staa CONTROL update the CONTROL byte WFI_END rts * *********************************************************** *********************************************************** * SUBROUTINE *********************************************************** * Name: Give Frank Information * Tag: GFI * * Purpose: * Take the value in PERIMETERS and translate the hardware * values to values that can be used by the traversal * algorithm. This is base on the current direction, * and then establishes the direction of each path by * using the sensor readings relative to that directin. * * Preconditions: None * * Memory Accessed: PERIMETERS, SCRATCH1, CURR_DIR, * MARK, CONTROL * * Registers Over-Written: A * * Register Values Left: * A -> updated CONTROL byte * * Subroutines Called: * GFI ldaa SCRATCH3 load PERIMETERS in A clr SCRATCH1 clear SCRATCH1 for use * the following branches will branch to one of the * four directions to package info for Frank brset CURR_DIR SOUTH_DIR S brset CURR_DIR WEST_DIR W brset CURR_DIR NORTH_DIR N brset CURR_DIR EAST_DIR E * set the path travelled in on automatically S bita #REAR_SENSOR is this bit set beq S_0 if not then go to next one * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 NORTH_DIR S_0 bita #FRONT_SENSOR is this bit set beq S_1 if not then go to next one * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 SOUTH_DIR S_1 bita #LEFT_SENSOR is this bit set beq S_2 if not then go to next one * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 EAST_DIR S_2 bita #RIGHT_SENSOR is this bit set beq S_END if not then go to send info * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 WEST_DIR S_END bra SEND * set the path travelled in on automatically W bita #REAR_SENSOR is this bit set beq W_0 if not then go to next one * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 EAST_DIR W_0 bita #FRONT_SENSOR is this bit set beq W_1 if not then go to next one * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 WEST_DIR W_1 bita #LEFT_SENSOR is this bit set beq W_2 if not then go to next one * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 SOUTH_DIR W_2 bita #RIGHT_SENSOR is this bit set beq W_END if not then go to send info * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 NORTH_DIR W_END bra SEND * set the path travelled in on automatically N bita #REAR_SENSOR is this bit set beq N_0 if not then go to next one * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 SOUTH_DIR N_0 bita #FRONT_SENSOR is this bit set beq N_1 if not then go to next one * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 NORTH_DIR N_1 bita #LEFT_SENSOR is this bit set beq N_2 if not then go to next one * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 WEST_DIR N_2 bita #RIGHT_SENSOR is this bit set beq N_END if not then go to send info * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 EAST_DIR N_END bra SEND * set the path travelled in on automatically E bita #REAR_SENSOR is this bit set beq E_0 if not then go to next one * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 WEST_DIR E_0 bita #FRONT_SENSOR is this bit set beq E_1 if not then go to next one * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 EAST_DIR E_1 bita #LEFT_SENSOR is this bit set beq E_2 if not then go to next one * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 NORTH_DIR E_2 bita #RIGHT_SENSOR is this bit set beq E_END if not then go to send info * set the bit in SCRATCH1 that corresponds to that dir bset SCRATCH1 SOUTH_DIR E_END bra SEND SEND ldaa SCRATCH1 load SCRATCH1 in A oraa #CONT_SW_MASK make sure I give control to * Frank staa MARK store A in MARK ldaa #GFI_TASK_SCH give control to WFI staa CONTROL update the CONTROL byte GFI_END rts * *********************************************************** *********************************************************** * SUBROUTINE *********************************************************** * Name: Check for Paths at Node * Tag: CPN * * Purpose: * Checks the available paths at a node by rotating a * specified number of steps to the left, then twice that * to the right, then left again to the original position. * Three SCRATCH variables are used to perform this task. * SCRATCH1 is used to hold the next state of the task. * SCRATCH2 is used to hold the number of steps remaining. * SCRATCH3 is used to hold the cumulative sensor reading. * The last function of this task is to update the CONTROL * byte--this ends this task and schedules the next task. * * Preconditions: * 1) store a #1 in SCRATCH1 * 2) store ROT_FOR_CORR in SCRATCH2 * 3) clear SCRATCH3 * * Memory Accessed: PERIMETERS, CONTROL, SCRATCH1, SCRATCH2, * SCRATCH3 * * Registers Over-Written: A, B * * Register Values Left: * A -> the updated CONTROL byte * B -> 0 * * Subroutines Called: * Rotate Left(RL) * Rotate Right(RR) * Read/Store Sensor Values(RSSV) * CPN ldaa SCRATCH1 load SCRATCH1 in A ldab SCRATCH2 load SCRATCH2 in B cmpa #1 if A = 1 then Rotate Left beq CPN_1 a little bit cmpa #2 if A = 2 then Rotate Right beq CPN_2 a little bit x2 cmpa #3 if A = 3 then Rotate Left beq CPN_3 a little bit again CPN_1 decb decrement the step counter in B beq CPN_11 if 0 then setup for Rotate Right stab SCRATCH2 store the new counter value jsr RL subroutine to Rotate Left jsr RSSV subroutine to Read/Store * Sensor Values ldaa SCRATCH3 load SCRATCH3 in A oraa PERIMETERS OR the latest PERIMETERS reading * with the cumulative value staa SCRATCH3 store the cumulative value * in SCRATCH3 bra CPN_END branch always CPN_11 ldaa #2 load Rotate Right(#2) in A staa SCRATCH1 store new mode in SCRATCH1 ldab #ROT_FOR_CORR set up the counter in SCRATCH2 lslb for Rotate Right that is twice stab SCRATCH2 what it was for Rotate Left bra CPN_END branch always CPN_2 decb decrement the step counter in B beq CPN_22 if 0 then setup for Rotate Left stab SCRATCH2 store the new counter value jsr RR subroutine to Rotate Right jsr RSSV subroutine to Read/Store * Sensor Values ldaa SCRATCH3 load SCRATCH3 in A oraa PERIMETERS OR the latest PERIMETERS reading * with the cumulative value staa SCRATCH3 store the cumulative value * in SCRATCH3 bra CPN_END branch always CPN_22 ldaa #3 load Rotate Left(#3) in A staa SCRATCH1 store new mode in SCRATCH1 ldab #ROT_FOR_CORR set up the counter in SCRATCH2 stab SCRATCH2 for Rotate Left to go back to * the original position bra CPN_END branch always CPN_3 decb decrement the step counter in B beq CPN_33 if 0 then setup for Ending Task stab SCRATCH2 store the new counter value jsr RL subroutine to Rotate Left jsr RSSV subroutine to Read/Store * Sensor Values ldaa SCRATCH3 load SCRATCH3 in A oraa PERIMETERS OR the latest PERIMETERS reading * with the cumulative value staa SCRATCH3 store the cumulative value * in SCRATCH3 bra CPN_END branch always CPN_33 ldaa SCRATCH3 put the new value in A staa PERIMETERS put the new value in PERIMETERS ldaa #CPN_TASK_SCH give control to GFI staa CONTROL update the CONTROL byte CPN_END rts * *********************************************************** *********************************************************** * SUBROUTINE *********************************************************** * Name: Rotate 180 Degrees at Node * Tag: UTURN * * Purpose: * To rotate 180 degrees to the left by first rotating * UT_CNTR steps, and then rotating until the front * sensor indicates that the path that Jerry arrived on * is under it. * * Preconditions: * 1) the UT_CNTR must be stored in SCRATCH4 * * Memory Accessed: CONTROL, SCRATCH4 * * Registers Over-Written: D (A & B) * * Register Values Left: * A -> the updated CONTROL byte(RLAN_TASK_SCH) * B -> junk * * Subroutines Called: * Rotate Left(RL) * Read/Store Sensor Values(RSSV) * UTURN ldd SCRATCH4 load the SCRATCH4 in D beq UTURN_1 start checking sensors * when the SCRATCH4 = 0 subd #1 decrement the SCRATCH4 std SCRATCH4 store the new SCRATCH4 jsr RL subroutine to Rotate Left bra UTURN_END branch always UTURN_1 jsr RSSV subroutine call to Read/ * Store Sensor Values ldaa PERIMETERS load PERIMETERS in A anda #FRONT_SENSOR and the Optimal Rotate Left * Sensor Mask from the reading * that was just taken bne UTURN_2 branch if last sensor input * is equal to the expected jsr RL subroutine to Rotate Left bra UTURN_END branch always UTURN_2 ldaa NEXT_DIR load the next direction in A staa CURR_DIR update the current direction ldd #FWD_CNTR satisfy preconditions std SCRATCH4 for Advance to Next Node ldaa #RAN_TASK_SCH give control to ANN staa CONTROL update the CONTROL byte UTURN_END rts * *********************************************************** *********************************************************** * SUBROUTINE *********************************************************** * Name: Rotate Left at Node * Tag: RLAN * * Purpose: * To rotate 90 degrees to the left by first rotating * ROT_CNTR steps, and then rotating until the left * sensor indicates that the path that Jerry arrived on * is under it. * * Preconditions: * 1) the ROT_CNTR must be stored in SCRATCH4 * * Memory Accessed: CONTROL, SCRATCH4 * * Registers Over-Written: D (A & B) * * Register Values Left: * A -> the updated CONTROL byte(RLAN_TASK_SCH) * B -> junk * * Subroutines Called: * Rotate Left(RL) * Read/Store Sensor Values(RSSV) * RLAN ldd SCRATCH4 load the SCRATCH4 in D beq RLAN_1 start checking sensors * when the SCRATCH4 = 0 subd #1 decrement the SCRATCH4 std SCRATCH4 store the new SCRATCH4 jsr RL subroutine to Rotate Left bra RLAN_END branch always RLAN_1 jsr RSSV subroutine call to Read/ * Store Sensor Values ldaa PERIMETERS load PERIMETERS in A anda #FRONT_SENSOR and the Optimal Rotate Left * Sensor Mask from the reading * that was just taken bne RLAN_2 branch if last sensor input * is equal to the expected jsr RL subroutine to Rotate Left bra RLAN_END branch always RLAN_2 ldaa NEXT_DIR load the next direction in A staa CURR_DIR update the current direction ldd #FWD_CNTR satisfy preconditions std SCRATCH4 for Advance to Next Node ldaa #RAN_TASK_SCH give control to ANN staa CONTROL update the CONTROL byte RLAN_END rts * *********************************************************** *********************************************************** * SUBROUTINE *********************************************************** * Name: Rotate Right at Node * Tag: RRAN * * Purpose: * To rotate 90 degrees to the right by first rotating * ROT_CNTR steps, and then rotating until the right * sensor indicates that the path that Jerry arrived on * is under it. * * Preconditions: * 1) the ROT_CNTR must be stored in SCRATCH4 * * Memory Accessed: CONTROL, SCRATCH4 * * Registers Over-Written: D (A & B) * * Register Values Left: * A -> the updated CONTROL byte(RRAN_TASK_SCH) * B -> junk * * Subroutines Called: * Rotate Right(RR) * Read/Store Sensor Values(RSSV) * RRAN ldd SCRATCH4 load the SCRATCH4 in D beq RRAN_1 start checking sensors * when the SCRATCH4 = 0 subd #1 decrement the SCRATCH4 std SCRATCH4 store the new SCRATCH4 jsr RR subroutine to Rotate Right bra RRAN_END branch always RRAN_1 jsr RSSV subroutine call to Read/ * Store Sensor Values ldaa PERIMETERS load PERIMETERS in A anda #FRONT_SENSOR and the Optimal Rotate Right * Sensor Mask from the reading * that was just taken bne RRAN_2 branch if last sensor input * is equal to the expected jsr RR subroutine to Rotate Right bra RRAN_END branch always RRAN_2 ldaa NEXT_DIR load the next direction in A staa CURR_DIR update the current direction ldd #FWD_CNTR satisfy preconditions std SCRATCH4 for Advance to Next Node ldaa #RAN_TASK_SCH give control to ANN staa CONTROL update the CONTROL byte RRAN_END rts * *********************************************************** *********************************************************** * SUBROUTINE *********************************************************** * Name: Advance to Next Node * Tag: ANN * * Purpose: * To advance to the next node by first stepping * FWD_CNTR steps, and then stepping until the center * sensor indicate that the path that Jerry arrived on * is under it by matching the CENTER_SENSOR mask. * * Preconditions: * 1) the FWD_CNTR must be stored in SCRATCH4 * * Memory Accessed: CONTROL, SCRATCH4 * * Registers Over-Written: D (A & B) * * Register Values Left: * A -> the updated CONTROL byte(ANN_TASK_SCH) * B -> junk * * Subroutines Called: * Step Forward(SF) * Read/Store Sensor Values(RSSV) * ANN ldd SCRATCH4 load the SCRATCH4 in D beq ANN_1 start checking sensors * when the SCRATCH4 = 0 subd #1 decrement the SCRATCH4 std SCRATCH4 store the new SCRATCH4 jsr SF subroutine to Step Forward bra ANN_END branch always ANN_1 jsr RSSV subroutine call to Read/ * Store Sensor Values suba #CENTER_SENSOR subtract the Optimal Forward * Sensor Mask from the reading * that was just taken beq ANN_2 branch if last sensor input * is equal to the expected jsr SF subroutine to Step Forward bra ANN_END branch always ANN_2 ldaa #1 satisfy preconditions staa SCRATCH1 for Check Paths at Node ldaa #ROT_FOR_CORR staa SCRATCH2 clr SCRATCH3 ldaa #ANN_TASK_SCH give control to CPN staa CONTROL update the CONTROL byte ANN_END rts * *********************************************************** *********************************************************** * SUBROUTINE *********************************************************** * Name: Read and Store Sensor Values * Tag: RSSV * * Purpose: * Reads the Perimeter Sensors via PORTC and the Center * Sensors via PORTE and stores them in RAM. The Control * Switch is also read via PORTE and also put into RAM. * * Preconditions: None * * Memory Accessed: PERIMETERS, CONTROL, CENTERS * * Registers Over-Written: A * * Register Values Left: * A -> adjusted value of Center Sensors * * Subroutines Called: None * RSSV ldaa PORTC load PORTC in A coma 1's complement A anda #PER_SEN_MASK remove possible garbage staa PERIMETERS store Perimeter Sensor Values ldaa PORTE load PORTE in A coma 1's complement A anda #$1 remove garbage bne RSSV_1 if not 0 then set bclr #CONTROL CONT_SW_MASK clear bit 7 bra RSSV_2 branch always RSSV_1 bset #CONTROL CONT_SW_MASK set bit 7 RSSV_2 ldaa PORTE load PORTE in A coma 1's complement A lsra logical shift right of A anda #CENT_SEN_MASK remove garbage staa CENTERS store Center Sensor Values RSSV_END rts * *********************************************************** *********************************************************** * SUBROUTINE *********************************************************** * Name: Step Forward * Tag: SF * * Purpose: * Causes both motors to take one step forward. * * Preconditions: None * * Memory Accessed: motStepDir * * Registers Over-Written: A, X, Y * * Register Values Left: * A -> motStepDir for the Left Motor * X -> address of the Left Motor Task Control Block * Y -> address of the Right Motor Task Control Block * * Subroutines Called: * Update Motor Output(UMO) * Merge/Send Motor Commands(MSMC) * SF ldx #RMTCB load the address of the Right * Motor Task Control Block in X ldaa #CCW load the constant that represents * the CCW direction in A staa motStepDir,x store the Motor Step Direction jsr UMO subroutine call to Update * the Motor Output ldx #LMTCB load the address of the Left * Motor Task Control Block in X ldaa #CW load the constant that represents * the CW direction in A staa motStepDir,x store the Motor Step Direction jsr UMO subroutine call to Update * the Motor Output ldy #RMTCB load the address of the Right * Motor Task Control Block in Y jsr MSMC subroutine call to Merge/ SF_END rts * *********************************************************** *********************************************************** * SUBROUTINE *********************************************************** * Name: Rotate Right * Tag: RR * * Purpose: * Causes the Left Motor to take one step forward, while * causing the Right Motor to take one step backward. * * Preconditions: None * * Memory Accessed: motStepDir * * Registers Over-Written: A, X, Y * * Register Values Left: * A -> motStepDir for the Left Motor * X -> address of the Left Motor Task Control Block * Y -> address of the Right Motor Task Control Block * * Subroutines Called: * Update Motor Output(UMO) * Merge/Send Motor Commands(MSMC) * RR ldx #RMTCB load the address of the Right * Motor Task Control Block in X ldaa #CW load the constant that represents * the CW direction in A staa motStepDir,x store the Motor Step Direction jsr UMO subroutine call to Update * the Motor Output ldx #LMTCB load the address of the Left * Motor Task Control Block in X ldaa #CW load the constant that represents * the CW direction in A staa motStepDir,x store the Motor Step Direction jsr UMO subroutine call to Update * the Motor Output ldy #RMTCB load the address of the Right * Motor Task Control Block in Y jsr MSMC subroutine call to Merge/ RR_END rts * *********************************************************** *********************************************************** * SUBROUTINE *********************************************************** * Name: Rotate Left * Tag: RL * * Purpose: * Causes the Left Motor to take one step backward, while * causing the Right Motor to take one step forward. * * Preconditions: None * * Memory Accessed: motStepDir * * Registers Over-Written: A, X, Y * * Register Values Left: * A -> motStepDir for the Left Motor * X -> address of the Left Motor Task Control Block * Y -> address of the Right Motor Task Control Block * * Subroutines Called: * Update Motor Output(UMO) * Merge/Send Motor Commands(MSMC) * RL ldx #RMTCB load the address of the Right * Motor Task Control Block in X ldaa #CCW load the constant that represents * the CCW direction in A staa motStepDir,x store the Motor Step Direction jsr UMO subroutine call to Update * the Motor Output ldx #LMTCB load the address of the Left * Motor Task Control Block in X ldaa #CCW load the constant that represents * the CCW direction in A staa motStepDir,x store the Motor Step Direction jsr UMO subroutine call to Update * the Motor Output ldy #RMTCB load the address of the Right * Motor Task Control Block in Y jsr MSMC subroutine call to Merge/ * Send Motor Commands RL_END rts * *********************************************************** *********************************************************** * SUBROUTINE *********************************************************** * Name: Update Motor Output * Tag: UMO * * Purpose: * To create the next output/command for the motors using * the Motor Output Value Table and motOutRefNum. The new * motOutComm and motOutRefNum are then stored for future use. * The motOutComm's are then masked and stored. * * Preconditions: * 1) The Motor Direction must be set appropriately. * 2) Load the appropriate Motor Task Control Block into X. * * Memory Accessed: motOutRefNum, motOutComm, motOutMask, * motStepDir * * Registers Over-Written: A, B, Y * * Register Values Left: * A -> motStepDir * B -> motOutComm * Y -> ROM address of the last Motor Output Value * * Subroutines Called: None * UMO ldab motOutRefNum,x reference to the last Motor * Output Command in B ldy #MOVT put the address of the Motor * Output Value Table in Y ldaa motStepDir,x direction of motor in A bne UMO_1 branch if motStepDir != 0 incb increment B bra UMO_2 branch always UMO_1 decb decrement B UMO_2 andb #$3 mask/loop for coded output stab motOutRefNum,x store the motOutRefNum aby add B to the address in y ldab 0,y load the Motor Output Value in B andb motOutMask,x mask the output for this motor stab motOutComm,x store the motOutComm UMO_END rts * *********************************************************** *********************************************************** * SUBROUTINE *********************************************************** * Name: Merge/Send Motor Commands * Tag: MSMC * * Purpose: * Access the motOutComm's, OR them together, and send * them to the motors via PORTB. * * Preconditions: * 1) Load the Left Motor Task Control Block into X. * 2) Load the Right Motor Task Control Block into Y. * * Memory Accessed: motOutComm * * Registers Over-Written: A * * Register Values Left: * A -> the combined Motor Output Commands * X -> address of the Left Motor Task Control Block * Y -> address of the Right Motor Task Control Block * * Subroutines Called: None * MSMC ldaa motOutComm,x load the motOutComm in A oraa motOutComm,Y OR the motOutComm's from * both TCB's in A staa PORTB send the command to the * motors via PORTB MSMC_END rts * *********************************************************** *********************************************************** * TABLES and TEMPLATES *********************************************************** * Name: Motor Output Value Table * Tag: MOVT * * Purpose: * the table of values that determine the sequence of * values to be sent to the motor * MOVT fcb $AA alpha fcb $66 beta fcb $55 delta fcb $99 gamma * *********************************************************** * Name: RAM Initialization Template * Tag: RT * * Purpose: * the initialization template to be loaded into RAM * RT * template for Global Parameters fcb $40 Control -- Startup Delay fcb $04 Current Direction fcb $00 Next Direction fcb $00 Perimeter Sensor values fcb $00 Center Sensor values fcb $80 Marks info to Frank byte fcb $00 Franks info to Mark byte fcb $FF the startup delay counter * template for Left Motor fcb $00 Direction of step fcb $00 Motor Output Reference Number fcb $A0 Motor Output Command(bit pattern) fcb $F0 Motor Output Mask * template for Right Motor fcb $00 Direction of step fcb $00 Motor Output Reference Number fcb $0A Motor Output Command(bit pattern) fcb $0F Motor Output Mask * scratch variables fcb $00 Scratch variable #1 fcb $00 Scratch variable #2 fcb $00 Scratch variable #3 fdb $0000 Scratch variable #4 fcb $00 ND2INDX fcb $08 CNTRLFRNK fcb $00 CNODE fcb $00 PRVNDE fcb $00 NXTNDE fcb $00 FNODE fcb $00 YCOORD fcb $00 XCOORD fdb $DDFF ALGSTK fdb $DCFF TBLSTK *********************************************************** * the following two bytes hold an address of * (index to) the data in the TEST maze TC fdb $0000 * the following is the data for the TEST maze * * NOTE: don't forget to pre-initialize a current direction * TEST fcb $84 North fcb $88 East fcb $81 South fcb $88 East fcb $84 North fcb $84 North fcb $82 West fcb $82 West fcb $84 North fcb $88 East fcb $88 East fcb $81 South fcb $88 East fcb $88 East fcb $84 North fcb $84 North fcb $82 West fcb $84 North fcb $82 West fcb $81 South fcb $84 North fcb $82 West fcb $81 South fcb $82 West fcb $84 North fcb $81 South fcb $81 South fcb $81 South fcb $00 *********************************************************** ********************************************************************** * Screen Messages * The screen I/O stuff is initialzed below. ********************************************************************** * MSG0 fcb #$1B fcb #$5B fcb #$32 fcb #$4A fcb #$04 MSG01 fcb #$1B fcb #$5B fcb #$1 fcb #$3B fcb #$1 fcb #$48 fcb #$04 MSG1 fcc 'Please Enter the Starting X Location (0-5): ' fcb #$04 MSG2 fcc 'Please Enter the Starting Y Location (0-5): ' fcb #$04 MSG2B fcc 'Please Enter the Starting Direction (e,n,w,s): ' fcb #$04 MSG3 fcc 'Please Enter the Destination X Location (0-5): ' fcb #$04 MSG4 fcc 'Please Enter the Destination Y Location (0-5): ' fcb #$04 MSG5 fcc 'Are these values correct (y/n)? ' fcb #$04 MSG6 fcc 'Disconnect Power and Terminal Lines from Jerry, ' fcc 'set him down on the correct' fcb #$04 MSG7 fcc 'node and flip the CTRL & MTR Switches when you are ready ' fcc 'to begin!!!' fcb #$04 END_P end