; Lab 5 (ele538) "Robot Roaming Program"
; BY: !!! PUT YOUR NAME HERE !!!

;; @module lab5 (Robot Roaming Program)
;; This implements all 3 requirements of Lab 5.
;; Once downloaded, do the following:
;; <dl>
;;    <li>To run on a system connected to a REAL eebot, do:
;;        <pre>
;;          GO 6000
;;        </pre>
;;    </li>
;;
;;    <li>To run on a system without an  eebot (a <em>vebot</em>), do:
;;        <pre>
;;          GO 6010
;;        </pre>
;;    </li>
;; </dl>
;;    @author PUT YOUR NAME HERE
;;    @version 1.0 (Oct 5, 2001)

;****************************************************
;*       EQUATES
;****************************************************

;******   Define Events 
TIME_OUT_EVT   EQU   0
FWD_BUMP_EVT   EQU   TIME_OUT_EVT+1
REAR_BUMP_EVT  EQU   FWD_BUMP_EVT+1
TEND_EVT       EQU   REAR_BUMP_EVT+1

;******   Define States 
START          EQU   0
FORWARD        EQU   1
REVERSE        EQU   2
REV_TURN       EQU   3
FWD_TURN       EQU   4
STOP           EQU   5

;******   Misc constants
SECONDS        EQU   15         ;number of TOF interrupts per second (aprrox.)

;******   Define Time outs
TALARM_FWD     EQU  2*SECONDS  ;Max time (in 1/15 secs) for FORWARD state
TALARM_START   EQU  4          ;Max time (in 1/15 secs) for START state
TALARM_REV     EQU  3*SECONDS  ;Max time (in 1/15 secs) for REVERSE state
TALARM_REV_TRN EQU  3*SECONDS  ;Max time (in 1/15 secs) for REV_TURN state
TALARM_FWD_TRN EQU  3*SECONDS  ;Max time (in 1/15 secs) for FWD_TURN state

;**********************************
;        MODIFIABLE DATA SEGMENT (must be in RAM)
;**********************************

;*****  Soft vectors first  *****
        org TOF_SOFT_VECTOR
        fdb tof_ISR       ;Soft vector for TOF interrupt

;*****  Ordinary modifiable RAM DATA SEGMENT starts here
        org $7000		; !!Use whatever ORG you want


;**********************************
;        READ-ONLY DATA SEGMENT (may be in ROM)
;**********************************

;******   Set names for states
startName     fcc 'Start'
              fcb 0
forwardName   fcc 'Forward'
              fcb 0
reverseName   fcc 'Reverse'
              fcb 0
revTurnName   fcc 'Reverse Turn'
              fcb 0
fwdTurnName   fcc 'Forward Turn'
              fcb 0
stopName      fcc 'Stop'
              fcb 0

;*****    Define State Transition diagram and actions as DATA

;** The general format is
; <state-name>_desc  next state number for TIME_OUT_EVT,
;                    next state number for FWD_BUMP_EVT,
;                    next state number for REAR_BUMP_EVT,
;                    next state number for TEND_EVT,
;                    timeout in units of 1/15 seconds (unsigned 8 bits)
;                    address of subroutine to call when entered (16 bits)
;                    address of state's name (null-terminated string)(16 bits) 
;
;   In general, each state descriptor requires
;     
;           Nevents bytes  (where Nevents = number of possible events)
;        +  1 byte   (for time out parameter)
;        +  2 bytes  (for address of init routine)
;        +  2 bytes  (for address of name string)
;       ------------------------------------------
;         Nevents+5 bytes
;
;     In this case, there are 4 possible events,
;     so each state descriptor requires 9 bytes.
;
;     The total number of bytes for the descriptors for "Nstates" is:
;          Nstates * (Nevents+5)
;     For this lab, there are 6 states, hence:
;          StateDescriptorOverhead (ROMable) = 6*(4+5) = 54 bytes
;
;     An auxiliary table to convert state numbers (8 bits) into
;     stateDescriptor pointers is also used.  This table contains
;     the address of each state descriptor.  Since addresses are
;     16 bits (2 bytes), this table needs Nstates*2 bytes of memory.
;
;     For this lab, there are 6 states; hence, the total amount of
;     ROMable memory for the state descriptors and address table
;     is:
;          StateDescriptorOverhead + 6*2 = 54 + 12 = 66 bytes
;***** Define offsets to fields in Start description struct.
NEXT_ON_TIME_OUT   equ 0
NEXT_ON_FWD_BUMP   equ 1
NEXT_ON_REAR_BUMP  equ 2
NEXT_ON_TEND       equ 3
TIME_OUT           equ 4
INIT_FUNC_PTR      equ 5
STATE_NAME         equ 7
         

START_desc
             fcb FORWARD        ;next State if time out
             fcb FORWARD        ;next State if Front collision
             fcb FORWARD        ;next State if Rear collision
             fcb STOP           ;next State if TEND (global time out)
             fcb TALARM_START   ;maximum duration in state
             fdb doStart        ;function pointer on entering state
             fdb startName      ;pointer to state name (string)

FORWARD_desc
             fcb FWD_TURN       ;next State if time out
             fcb REVERSE        ;next State if Front collision
             fcb FORWARD        ;next State if Rear collision
             fcb STOP           ;next State if TEND (global time out)
             fcb TALARM_FWD     ;maximum duration in state
             fdb doForward      ;function pointer on entering state
             fdb forwardName    ;pointer to state name (string)

REVERSE_desc 
             fcb REV_TURN       ;next State if time out
             fcb REVERSE        ;next State if Front collision
             fcb REVERSE        ;next State if Rear collision !!!!!!!!
             fcb STOP           ;next State if TEND (global time out)
             fcb TALARM_REV     ;maximum duration in state
             fdb doReverse      ;function pointer on entering state
             fdb reverseName    ;pointer to state name (string)


REV_TURN_desc 
             fcb FORWARD        ;next State if time out
             fcb REV_TURN       ;next State if Front collision
             fcb REV_TURN       ;next State if Rear collision !!!!!!!!
             fcb STOP           ;next State if TEND (global time out)
             fcb TALARM_REV_TRN ;maximum duration in state
             fdb doRevTurn      ;function pointer on entering state
             fdb revTurnName    ;pointer to state name (string)


FWD_TURN__desc 
             fcb FORWARD        ;next State if time out
             fcb FWD_TURN       ;next State if Front collision !!!!!!!!
             fcb FWD_TURN       ;next State if Rear collision
             fcb STOP           ;next State if TEND (global time out)
             fcb TALARM_FWD_TRN ;maximum duration in state
             fdb doFwdTurn      ;function pointer on entering state
             fdb fwdTurnName    ;pointer to state name (string)


STOP_desc 
             fcb STOP           ;next State if time out
             fcb STOP           ;next State if Front collision
             fcb STOP           ;next State if Rear collision
             fcb STOP           ;next State if TEND (global time out)
             fcb 0              ;maximum duration in state
             fdb doStop         ;function pointer on entering state
             fdb stopName       ;pointer to state name (string)


stateTable  fdb START_desc
            fdb FORWARD_desc
            fdb REVERSE_desc
            fdb REV_TURN_desc
            fdb FWD_TURN__desc
            fdb STOP_desc

;****************************************************
;*       PROGRAM SEGMENT (may be in ROM)
;****************************************************
         org $6000		; !!!!OR WHATEVER YOU WANT!!!
	jsr Initialization      ;  whatever you want to do
        ldaa #START
        ldx  #START_desc
        jsr doStateMachine ;NEVER returns! (but may exit)



Initialization:			;  Just a STUB!!!!
        rts
	
;; @name doStateMachine
;; Implements the state transition diagram.  The
;; subroutine loops forever; i.e. it never returns <em>normally</em>.
;; However, in most cases, at least one state (such as one called
;; STOP) results in returning control to the Monitor (or operating
;; system) with an SWI instruction.  Note that on entry, it is assumed
;; that the state has <em>not</em> been initialized.  This routine
;; first initiales the initial state; subsequent transitions that
;; result in a different state automatically invoke the state specific
;; initializations.
;;
;; @param AccA The initial non-initialized state(state number)
;; @param RegX Pointer to current state descriptor

doStateMachine:			; YOU!!! have to implement the stub

        rts


;; @name getEvent
;; Checks for events quickly.  If an event is found, its ID is returned;
;; otherwise -1 is returned.  The first event found is returned.  The
;; order of checking for events is:
;;  <ol>
;;    <li> TEND_EVT event (i.e. global time out)
;;    <li> TIME_OUT_EVT event (i.e. state time out)
;;    <li> FWD_BUMP_EVT event. (i.e. front collision)
;;    <li> REAR_BUMP_EVT event. Ii.e. rear collision)
;;  </ol>
;; @param none
;; @return AccB = event number or -1 if no event
;; @return CC N bit set if no event
getEvent:			; YOU!! have to implement this
	;First, check for TEND event

        ;Second, check for state time out

	;Third and fourth, check for collisions

	rts





doStart:			; JUST a STUB YOU have to implement it
       rts

doStop:
	swi
	rts

doFwdTurn:			; JUST a STUB YOU have to implement it
	
	rts

doForward:			; JUST a STUB YOU have to implement it
	rts

doRevTurn:			; JUST a STUB YOU have to implement it
	rts

doReverse:			; JUST a STUB YOU have to implement it
	rts

        end


