;;; PLEASE NOTE that this is assembly code.
;;;
;;; It is formated in a lisp-like syntax for a home brew assembler
;;; written by Professor Brooks.

;;; Full code for robot behavior,
;;; including simple collision avoidance
;;;
;;; Charles Rosenberg
;;;         Start: 5/6/89
;;; Last modified: 5/7/89
;;; Modified 5/25/89 for new motor relay arrangement
;;; Modified 11/5/90 to move all variables from NVRAM into RAM

(defprog doit
  :machine 6811
  :start #xE000
  :code ((=v porta  #x2000)   ; port data for a (i/o)
         (=v portb  #x2004)   ; port data for b (output only)
         (=v portc  #x2003)   ; port data for c (i/o)
         (=v portcl #x2005)   ; port data for c (i/o latched)
         (=v ddrc   #x2007)   ; data direction for port c, 0=in/1=out
         (=v portd  #x2008)   ; port data for d (i/o)
         (=v porte  #x200A)   ; port data for e (input only)
         (=v init   #x103D)   ; ram and i/o mapping register
         (=v tflg2  #x2025)   ; misc. interrupt flags
         (=v toc1   #x2016)   ; timer output compare register #1
         (=v toc2   #x2018)   ; timer output compare register #2
         (=v oc1d   #x200D)   ; output compare 1 data register
         (=v tctl1  #x2020)   ; timer control register 1
         (=v oc1m   #x200C)   ; output compare 1 mask register
         (=v option #x2039)  ; system configuration options
         (=v adctl  #x2030)  ; a/d control register
         (=v ad0    #x2031)  ; output of a/d number 0
         (=v ad1    #x2032)  ; output of a/d number 1
         (=v ad2    #x2033)  ; output of a/d number 2
         (=v ad3    #x2034)  ; output of a/d number 3
         (=c stack  #xFF)

         (=v head_pyr ad0)    ; pyro detector on head
         (=v arm_pyr ad1)     ; pyro detector on the arm

         (=c mux_off #x20)    ; code to disable all mux outputs
         (=c mux0   #x00)     ; code to enable mux0
         (=c mux1   #x40)     ; code to enable mux1
         (=c mux2   #x80)     ; code to enable mux2
         (=c mux3   #xC0)     ; code to enable mux3

         (=c ir_mux mux0)     ; infrared sensor is connected to mux0
         (=c lim_mux mux1)    ; limit switches are connected to mux1
         (=c mot_mux mux2)    ; motor controller is connected to mux2
         (=c eye_mux mux3)    ; "eyes" are connected to mux3

         (=c all_in  #x00)    ; code to set port to all inputs
         (=c all_out #xFF)    ; code to set port to all outputs
            


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;       
     start
         (lds ! stack)       ; initialize the stack pointer
         (jsr setup)         ; set up all the variables and registers

     loop
         (jsr arm_maint)       ; maintain arm position for travelling

         (jsr update_time)     ; update the time counters

         (jsr scan_servo)      ; keep the servo scanning

         (jsr ir_read)         ; read the ir sensors
         (tstb)                ; go forward for no hit
         (beq no_hit)          
         (jsr parse_hit)       ; figure out what to do with collision

     no_hit
         (jsr check_pyros)     ; see if the pyros sensed something

         (ldaa mot_mode)       ; check the motion mode
         (beq reg_mot)
         (jsr mode_handle)
         (bra loop)

     reg_mot
         (jsr move_for)
         (bra loop)




; routine to parse collision and figure out what to do

     parse_hit
         (clr hit_l)           ; clear out the hit flags
         (clr hit_r)
         (clr hit_f)
         (clr hit_b)
         (clr no_fl)
         (ldaa ! #x01)          ; flag value

         ; set the hit flags
         (bitb ! #b10000000)   ; is floor missing?
         (beq nxta)          
         (staa no_fl)

     nxta
         (bitb ! #b00010010)    ; hit on the left?
         (beq nxtb)
         (staa hit_l)

     nxtb
         (bitb ! #b00100100)    ; hit on the right?
         (beq nxtc)
         (staa hit_r)

     nxtc
         (bitb ! #b01001000)    ; hit on the front?
         (beq nxtd)
         (staa hit_f)

    nxtd
         (bitb ! #b00000001)    ; hit on the back?
         (beq hit_look)
         (staa hit_b)

    ; examine the "hits" and decide what to do
    ;    L^R^F^B = no go
    ;      floor = B
    ;      L^R^F = B
    ;        L^R = F or B
    ;        F^R = full-L
    ;          R = full-L
    ;        F^L = full-R
    ;          L = full-R
    ;          F = full-R or full-L
    
    hit_look
         (ldaa hit_l)          ; check for L^R^F^B
         (anda hit_r)
         (anda hit_f)
         (anda hit_b)
         (beq chka)
     dont_go
         (ldaa ! #x03)        ; if surrounded stop, mode three

     mode_set
         (staa mot_mode)
         (clr mot_time)      ; zero motor time
         (clr mot_cnt)       ; clear motor state
         (rts)

    chka
         (ldaa no_fl)        ; check for no floor
         (beq chkb)
         (ldaa hit_b)         ; if blocked in the back, stop
         (bne dont_go)
         (ldaa ! #x04)       ; if surrounded back up, mode four
         (jmp mode_set)

    chkb
         (ldaa hit_l)          ; check for L^R^F
         (anda hit_r)
         (anda hit_f)
         (beq chkc)
         (ldaa ! #x04)        ; if boxed in, back up, mode four
         (staa last_lr)
         (jmp mode_set)

    chkc
         (ldaa hit_l)          ; check for L^R
         (anda hit_r)
         (beq chkd)
         (ldaa hit_b)        ; if already in backup & no hit_b, continue
         (bne do_for)
         (ldaa last_lr)     
         (cmpa ! #x04)
         (beq go_set)
     do_for
         (ldaa ! #x05)       ; otherwise go foward
     go_set
         (staa last_lr)
         (jmp mode_set)

    chkd 
         (ldaa hit_r)         ; check for F^R
         (beq chke)
         (ldaa ! #x01)        ; go to motion mode 1, turn left
         (staa mot_mode)
         (staa last_mode)
         (rts)

    chke 
         (ldaa hit_r)         ; check for R
         (beq chkf)
         (ldaa ! #x01)        ; go to motion mode 1, turn left
         (staa mot_mode)
         (staa last_mode)
         (rts)

    chkf
         (ldaa hit_f)         ; check for F^L, turn right
         (anda hit_l)
         (beq chkg)
         (ldaa ! #x02)        ; go to motion mode 2
         (staa mot_mode)
         (staa last_mode)
         (rts)

    chkg
         (ldaa hit_l)         ; check for L, turn right
         (beq chkh)
         (ldaa ! #x02)        ; go to motion mode 2
         (staa mot_mode)
         (staa last_mode)
         (rts)

     chkh
                              ; for just forward use last_mode, left or right
         (ldaa last_mode)     
         (staa mot_mode)
         (rts)


; routine to wait for a long time
     delaylots
         (ldx ! #x1000)
     dlots
         (iterate ((i 12)) (nop))
         (dex)
         (bne dlots)
         (rts)


; routine to setup all of the initial state of the program
     setup
         (ldaa ! #x02)       ; move register block to 0x2000
         (staa init)

         (ldaa ! mux_off)    ; turn off the mux
         (staa portb)
         
         (jsr stop_all)      ; turn off all the motors
         (clr eye_state)     ; turn the eyes on
         (jsr eyes_on)     

         (clr mot_time)      ; zero motor time
         (clr mot_cnt)       ; clear motor state
         (clr mot_mode)      ; start out with regular motion
         (ldaa ! #x02)       ; start out so forward turns right
         (staa last_mode)
         (ldaa ! #x05)       ; start out so L^R block goes forward
         (staa last_lr)

         ; set up for driving the servo motor
         ; the output to the motor is on oc2
         (ldab ! #b01000000)  ; this will allow oc1 bit#6 (oc2) to be high on suc. oc1 
         (stab oc1d)
         (ldd ! #x0001)       ; set the oc reg to be 1
         (std toc1)
         (ldd ! #x0100)       ; start servo out at all right position
         (std toc2)
         (ldab ! #b10000000)  ; this determines that when oc2 comp. success, gnd the line oc2
         (stab tctl1)
         (ldab ! #b01000000)  ; this enables us to raise the line oc2 when oc1 compares successfully
         (stab oc1m)
         (clr scan_time)      ; set timers ou at zero
         (clr servo_time)
         (clr servo_mode)     ; start servo out in 0, all right mode

         ; set up for reading the pyros
         (ldaa ! #x90)       ; set a/d to "on" in system configuration
         (staa option)
         (ldaa ! #x30)       ; set up to scan a/d channels 0-3 continuously
         (staa adctl)
         (clr head_fl)       ; clear arm and head pyro flags
         (clr arm_fl)
         (ldaa ! #x01)       ; set first time flag so first reading is ignored
         (staa first_fl)
         (clr person_pos)    ; default person position for unsure mode is to left
         (clr arm_dir)       ; default arm moves up when adjusting for scan
         (clr pyro_time)     ; clear the pyro time
         (clr off_time)      ; turn off pyro supression
         (clr dont_shoot)    ; clear don't shoot
         (jsr lim_read)      ; if arm up and down are on then don't shoot
         (andb ! #x09)
         (cmpb ! #x09)
         (bne exit_setup)
         (ldaa ! #x01)       ; don't shoot on
         (staa dont_shoot)

      exit_setup
         (rts)


; routine to read the ir sensors into acc b
     ir_read
         (ldaa ! all_in)       ; set up portc as all inputs
         (staa ddrc)
         (ldaa ! ir_mux)       ; set the mux to out0 to enable the ir sensors
         (staa portb)
         (ldab portc)          ; get the IR data
         (ldaa ! mux_off)      ; turn off the mux
         (staa portb)
         (eorb ! #x80)         ; invert floor sensor hit bit, so 1 is active
         (andb ! #x7F)         ; for now and out the floor, front and back sensor
         (stab ir_val)         ; store the value
         (rts)


; routine to read the limit switches into acc b
     lim_read
         (ldaa ! all_in)       ; set up portc as all inputs
         (staa ddrc)
         (ldaa ! lim_mux)       ; set the mux to out0 to enable the ir sensors
         (staa portb)
         (ldab portc)          ; get the lim data
         (ldaa ! mux_off)      ; turn off the mux
         (staa portb)
         (stab lim_val)         ; store the value

         ; set up/dwn arm direction
         (bitb ! #x01)    ; if arm is down, set adjust dir to up
         (beq arm_not_dwn)
         (clr arm_dir)
         (rts)

     arm_not_dwn
         (bitb ! #x08)    ; if arm is up, set adjust to dwn
         (beq lim_read_done)
         (ldaa ! #x01)
         (staa arm_dir)

     lim_read_done
         (rts)


; routines to set the motors/robot moving in a particular direction

     move_for
         (ldaa mot_state)      ; get current motor register state
         (oraa ! #b01110000)  ; set the appropriate bits
         (anda ! #b11101111)

     set_mot
         (ldab ! all_out)     ; set up portc as all outputs
         (stab ddrc)
         (staa mot_state)     ; save  the new motor state
         (staa portc)         ; execute the change
         (ldaa ! mot_mux)
         (staa portb)
         (ldaa ! mux_off)
         (staa portb)
         (rts)
          
     move_back
         (ldaa mot_state)     ; get current motor register state
         (anda ! #b10001111)   ; set the appropriate bits
         (bra set_mot)        ; execute the change

     turn_left
         (ldaa mot_state)     ; get current motor register state
         (oraa ! #b01110000)  ; set the appropriate bits
         (anda ! #b11001111)
         (bra set_mot)        ; execute the change

     turn_right
         (ldaa mot_state)     ; get current motor register state
         (oraa ! #b01110000)  ; set the appropriate bits
         (anda ! #b10101111)
         (bra set_mot)        ; execute the change

     wrist_up
         (ldaa mot_state)     ; get current motor register state
         (anda ! #b11111001)   ; set the appropriate bits
         (bra set_mot)        ; execute the change
   
     wrist_dwn
         (ldaa mot_state)     ; get current motor register state
         (oraa ! #b00000110)   ; set the appropriate bits
         (bra set_mot)        ; execute the change

     arm_up
         (ldaa mot_state)     ; get current motor register state
         (oraa ! #b00001001)  ; set the appropriate bits
         (bra set_mot)        ; execute the change

     arm_dwn
         (ldaa mot_state)     ; get current motor register state
         (anda ! #b11110110)   ; set the appropriate bits
         (bra set_mot)        ; execute the change

     shoot_gun
         (ldaa mot_state)     ; get current motor register state
         (anda ! #b01111111)   ; set the appropriate bits
         (bra set_mot)        ; execute the change
   
     stop_move
         (ldaa mot_state)     ; get current motor register state
         (oraa ! #b01110000)   ; set the appropriate bits
         (bra set_mot)

     stop_wrist
         (ldaa mot_state)     ; get current motor register state
         (oraa ! #b00000110)   ; set the appropriate bits
         (anda ! #b11111011)
         (bra set_mot)

     stop_arm
         (ldaa mot_state)     ; get current motor register state
         (oraa ! #b00001001)   ; set the appropriate bits
         (anda ! #b11110111)
         (bra set_mot)

     stop_gun
         (ldaa mot_state)     ; get current motor register state
         (oraa ! #b10000000)   ; set the appropriate bits
         (bra set_mot)

     stop_all
         (ldaa ! #b11110011)     ; set the motor state to all off
         (bra set_mot)



; routines to set the eyes on/off 

     eyes_on
         (ldaa eye_state)      ; get current eye register state
         (anda ! #b11111001)   ; set the appropriate bits

     set_eyes
         (ldab ! all_out)     ; set up portc as all outputs
         (stab ddrc)
         (staa eye_state)     ; save the new eye state
         (staa portc)         ; execute the change
         (ldaa ! eye_mux)
         (staa portb)
         (ldaa ! mux_off)
         (staa portb)
         (rts)
          
     eyes_off
         (ldaa eye_state)     ; get current motor register state
         (oraa ! #b00000110)   ; set the appropriate bits
         (bra set_eyes)        ; execute the change
          
     lft_eye_on
         (ldaa eye_state)     ; get current motor register state
         (anda ! #b11111011)   ; set the appropriate bits
         (bra set_eyes)        ; execute the change
          
     lft_eye_off
         (ldaa eye_state)     ; get current motor register state
         (oraa ! #b00000100)   ; set the appropriate bits
         (bra set_eyes)        ; execute the change
          
     rt_eye_on
         (ldaa eye_state)     ; get current motor register state
         (anda ! #b11111101)   ; set the appropriate bits
         (bra set_eyes)        ; execute the change
          
     rt_eye_off
         (ldaa eye_state)     ; get current motor register state
         (oraa ! #b00000010)   ; set the appropriate bits
         (bra set_eyes)        ; execute the change


; routine to scan servo back and forth
     scan_servo
         (tst dont_shoot)     ; if don't shoot mode exit
         (bne servo_exit)

         (ldab servo_time)    ; get the timing information
         (ldaa servo_mode)    ; get the current servo mode
         (bne smode1)
         (cmpb ! #x05)        ; has enough time gone by to get to all right?
         (bls servo_exit)
         (ldd ! #x0880)       ; set mid-right servo position

     set_smode
         (inc servo_mode)     ; go to new servo mode
     set_smodea
         (std toc2)           ; store new servo position
         (clr servo_time)     ; reset the servo_timer
     servo_exit
          (rts)

     smode1
         (cmpa ! #x01)        ; is it mode 1?
         (bne smode2)
         (cmpb ! #x05)        ; has enough time gone by to get to mid right?
         (bls servo_exit)
         (ldd ! #x0A80)       ; set mid-left servo position
         (bra set_smode)

     smode2
         (cmpa ! #x02)
         (bne smode3)
         (cmpb ! #x02)        ; has enough time gone by to get to mid left?
         (bls servo_exit)
         (ldd ! #x1200)       ; set all_left servo position
         (bra set_smode)

     smode3
         (cmpa ! #x03)
         (bne smode4)
         (cmpb ! #x05)        ; has enough time gone by to get to all left?
         (bls servo_exit)
         (ldd ! #x0A80)       ; set mid-left servo position
         (bra set_smode)

     smode4
         (cmpa ! #x04)
         (bne smode5)
         (cmpb ! #x06)        ; has enough time gone by to get to mid left?
         (bls servo_exit)
         (ldd ! #x0880)       ; set mid-right servo position
         (bra set_smode)

     smode5
         (cmpb ! #x03)        ; has enough time gone by to get to mid right?
         (bls servo_exit)
         (ldd ! #x0100)       ; set all-right servo position
         (clr servo_mode)     ; reset servo mode to zero
         (bra set_smodea)




; routine to handle person detection etc
     check_pyros
         (tst dont_shoot) ; exit immediately in don't shoot mode
         (bne exit_pyro)
         (tst mot_mode)   ; if currerntly avoiding an obstacle, exit
         (bne exit_pyro)
         (tst off_time)   ; exit if supressed
         (bne exit_pyro)
         (tst head_fl)    ; if head sensed something then move arm into scan pos
         (bne got_one0)
     exit_pyro
         (jsr eyes_on)    ; make sure eyes are on
         (rts)

     supress_exit
         (jsr eyes_on)      
         (ldaa ! #x28)     ; supress behavior for four seconds
         (staa off_time)
         (rts)

     got_one0
         (clr arm_fl)      ; reset the arm flag
         (clr mot_mode)    ; start at zero motor mode
         (jsr stop_all)    ; stop all motion
         (clr adj_fl)      ; clear adjust flag
         (clr search_time) ; clear time out for search
         (clr head_fl)     ; clear the sense flag
         (ldaa person_mode); get the person first seen at
         (jmp found_enter) ; move body accordingly

     got_one
         (ldaa servo_mode)  ; keep track of starting scan mode
         (staa start_mode)
         (clr head_fl)     ; clear the sense flag
         
         ; wait until scan is not in start mode
     wait1
         (jsr adj_pos)     ; adjust the arm into scanning position         
         (jsr update_time)     ; update the time counters
         (ldaa search_time)    ; time out?
         (cmpa ! #x60)
         (beq supress_exit)
         (jsr blink_eyes)      ; alternately blink the eyes
         (jsr scan_servo)      ; keep the servo scanning
         (tst arm_fl)          ; found person, go on to next behavior
         (beq no_fnd1)
         (jmp arm_found)
     no_fnd1
         (tst head_fl)         ; break out of loop if person sensed
         (bne go_found_em)
         (ldaa servo_mode)
         (cmpa start_mode)
         (beq wait1)

         ; wait until scan is in start mode
     wait2
         (jsr adj_pos)     ; adjust the arm into scanning position         
         (jsr update_time)     ; update the time counters
         (ldaa search_time)    ; time out?
         (cmpa ! #x60)
     go_supress_exit
         (beq supress_exit)
         (jsr blink_eyes)      ; alternately blink the eyes
         (jsr scan_servo)      ; keep the servo scanning
         (tst arm_fl)          ; found person, go on to next behavior
         (beq no_fnd2)
         (jmp arm_found)
     no_fnd2
         (tst head_fl)         ; break out of loop if person sensed
     go_found_em
         (bne found_em)
         (ldaa servo_mode)
         (cmpa start_mode)
         (bne wait2)

         ; wait until scan is not in start mode
     wait3
         (jsr adj_pos)         ; adjust the arm into scanning position         
         (jsr update_time)     ; update the time counters
         (ldaa search_time)    ; time out?
         (cmpa ! #x60)
         (beq go_supress_exit)
         (jsr blink_eyes)      ; alternately blink the eyes
         (jsr scan_servo)      ; keep the servo scanning
         (tst arm_fl)          ; found person, go on to next behavior
         (beq no_fnd3)
         (jmp arm_found)
     no_fnd3
         (tst head_fl)         ; break out of loop if person sensed
         (bne found_em)
         (ldaa servo_mode)
         (cmpa start_mode)
         (beq wait3)

         ; wait until scan is in start mode
     wait4
         (jsr adj_pos)         ; adjust the arm into scanning position         
         (jsr update_time)     ; update the time counters
         (ldaa search_time)    ; time out?
         (cmpa ! #x60)
         (beq go_supress_exit)
         (jsr blink_eyes)      ; alternately blink the eyes
         (jsr scan_servo)      ; keep the servo scanning
         (tst arm_fl)          ; found person, go on to next behavior
         (beq no_fnd4)
         (jmp arm_found)
     no_fnd4
         (tst head_fl)         ; break out of loop if person sensed
         (bne found_em)
         (ldaa servo_mode)
         (cmpa start_mode)
         (bne wait4)

         (jsr stop_all)  ; turn everything off
         (jsr eyes_on)   ; keep those eyes on
         (rts)           ; person must be gone, go back to normal roam mode

     found_em
          (clr head_fl)        ; reset the found flag
          (ldaa servo_mode)    ; localize person position via scan position
     found_enter
          (beq rt_yes)         ; mode 0 or 1 is to the right
          (cmpa ! #x01) 
          (bne rt_no)
      rt_yes
          (clr person_pos)     ; 0 means to the right
          (bra scan_body)

      rt_no
          (cmpa ! #x03)        ; mode 3 or 4 is to the left
          (beq left_yes)
          (cmpa ! #x04)        
          (bne scan_body)      ; otherwise it is in the position it was last
      left_yes
          (ldaa ! #x01)        ; 1 means to the left
          (staa person_pos)

     scan_body
         (tst person_pos)     ; move body in direction of the person
         (beq goto_rt)
         (jsr turn_left)      ; turn left
         (jmp got_one)

     goto_rt
         (jsr turn_right)      ; turn to the right
         (jmp got_one)



     ; found person, shoot, blink eyes every .1 seconds
     arm_found
         (jsr stop_all)        ; stop all extraneous motion
         (jsr shoot_gun)       ; turn on the gun
         (clr head_fl)         ; clear the arm and head flag
         (clr arm_fl)
         (clr no_switch)       ; enable switch direction
         (clr off_time)        ; use to delay reaction to arm sight
         (ldaa ! #x1E)         ; set swing length to 3 seconds
         (staa swing_length)

         ; continue moving in this direction for 1 second
         (clr search_time)     ; use search time to time shooting 
         (tst person_pos)      ; move body in direction of the person
         (beq go_rt1)
         (jsr turn_left)       ; turn left
         (bra swing)
    go_rt1
         (jsr turn_right)      ; turn to the right

    swing
         (jsr adj_pos)         ; adjust the arm into scan/shoot position         
         (jsr update_time)     ; move time forward
         (jsr scan_servo)      ; keep the servo scanning

         (tst arm_fl)          ; did the arm see target?
         (beq arm_no_see)
         (tst off_time)        ; if switch already pending ignore
         (bne arm_no_see)
         (ldaa ! #x06)         ; delay direction switch for .5 seconds (has to be 6)
         (staa off_time)
         (clr arm_fl)          ; turn off the arm flag

     arm_no_see
         (tst head_fl)         ; did the head see it?
         (beq chk_time)
         (clr head_fl)         ; reset the flag

         (ldaa servo_mode)     ; localize person position via scan position
         (beq rt_yup)          ; mode 0 or 1 is to the right
         (cmpa ! #x01)  
         (bne rt_nope)
     rt_yup
         (clr person_pos)      ; 0 means to the right
         (ldaa ! #x32)         ; make swing length five seconds to compensate
         (bra do_yup)

     rt_nope
         (cmpa ! #x03)         ; mode 3 or 4 is to the left
         (beq left_yup)
         (cmpa ! #x04)        
         (bne chk_time)        ; otherwise it is in the position it was last
      left_yup
         (ldaa ! #x01)         ; 1 means to the left
         (staa person_pos)
     do_yup
         (ldaa ! #x32)         ; make swing length five seconds to compensate
         (staa swing_length)
         (bra set_dir)

     chk_time
         (ldaa off_time)      ; time to do the switch?
         (cmpa ! #x01)
         (beq inv_dir)
         (ldaa search_time)
         (cmpa ! #x0A)        ; if nothing seen, switch directions after 1 sec.
         (bne cont_swing)
         (tst no_switch)      ; switch supressed?
         (bne cont_swing)
     inv_dir
         (ldaa person_pos)     ; invert person_pos to switch direction
         (eora ! #x01)
         (staa person_pos)
     set_dir
         (tst person_pos)      ; set body direction
         (beq go_rt2)
         (jsr turn_left)       ; turn left
         (bra done_set)
     go_rt2
         (jsr turn_right)      ; turn to the right
     done_set
         (ldaa ! #x01)         ; turn off switching behavior
         (staa no_switch)
         (clr off_time)        ; turn off the switch behavior

     cont_swing
         (cmpa swing_length)   ; exit swing after certain time
         (beq done_shoot)
         (bita ! #x01)         ; use first bit to time eyes
         (bne on1)
         (jsr eyes_off)
         (jmp swing)
     on1
         (jsr eyes_on)
         (jmp swing)


     done_shoot
         (jsr stop_gun)    ; turn off the gun
         (jsr eyes_on)
         (ldaa ! #x46)     ; supress behavior for seven seconds
         (staa off_time)
         (rts)






; routine to get the arm in scanning position
     adj_pos
         (jsr lim_read)   ; read the limit switch
         (bitb ! #x10)    ; if arm isn't at mid, put it there
         (bne arm_ok) 

         (tst arm_dir)
         (bne adj_dwn)
         (jsr arm_up)     ; if 0 move arm up
         (bra adj_wrist)

     adj_dwn
         (jsr arm_dwn)    ; if 1 move arm down

     adj_wrist
         (tst adj_fl)     ; if wrist was already up, then tweak wrist
         (bne tweak_wrist)
         (ldaa lim_val)    ; if wrist is not up, move it there
         (bita ! #x02)
         (bne adj_done)
     adj_up
         (jsr wrist_up)   ; move the wrist up
         (rts)

     adj_done
         (ldaa lim_val)
         (bita ! #x10)      ; if arm isn't at mid then don't set flag
         (beq leave_adj)

         (ldaa ! #x01)      ; set adjust flag
         (staa adj_fl)
         (ldaa ! #x09)     ; set wrist adjust duration
         (staa adj_time)

     leave_adj
         (rts)
  
     arm_ok
         (jsr stop_arm)   ; arm is ok, stop it
         (bra adj_wrist)

     tweak_wrist
         (ldaa lim_val)   ; if wrist is down then turn off tweak
         (bita ! #x04)
         (beq tweak_ok)
         (clr adj_fl)
         (bra adj_up)

     tweak_ok
         (tst adj_time)   ; if arm at mid, do small downward adj to wrist once
         (bmi no_adj)
         (jsr wrist_dwn)
         (rts)

     no_adj
        (jsr stop_wrist)
        (rts)


; routine to blink eyes alternately left and right based on search_time
     blink_eyes
         (ldaa search_time)
         (bita ! #x02)         ; use second bit to time eyes
         (bne on_l)
         (jsr rt_eye_on)
         (jsr lft_eye_off)
         (rts)

     on_l
         (jsr lft_eye_on)
         (jsr rt_eye_off)
         (rts)




; routine to keep track of timing changes governed
; by the free runing counter, once every 32.77ms an increment is done

     update_time
        (ldaa tflg2)      ; get the timer flags
        (bita ! #x80)     ; test the overflow bit
        (bne do_time)
        (jmp time_done)

     do_time
        (ldaa ! #x80)     ; clear the overflow flag
        (staa tflg2)

        (ldaa mot_mode)   ; only set counters for irregular motion mode
        (beq cnt_scan)
        (inc mot_time)    ; increment motor time
        (ldaa mot_time)
        (ldab mot_cnt)    ; go shorter for count zero and one
        (stab last_cnt)   ; store this value of the cnt
        (cmpb ! #x01)
        (bhi no_cntz)
        (cmpa ! #x10)     ; half time for count zero and one
        (beq cnt_m)
        (bne cnt_scan)

    no_cntz
        (cmpa ! #x20)     ; time (2.5 sec) to do a switch?
        (bne cnt_scan)
    cnt_m
        (clr mot_time)    ; reset the motor counter
        (inc mot_cnt)     ; increment motor state
    
    cnt_scan
        (inc scan_time)   ; scan time is used to create a tick every .1 sec
        (ldaa scan_time)
        (cmpa ! #x03)
        (bne read_pyros)
        (clr scan_time)
        (inc servo_time)  ; servo time gets incremented every step
        (inc search_time) ; search time gets incremented also
        (tst off_time)    ; decrement off time if not zero
        (beq read_pyros)
        (dec off_time)


    read_pyros
        (inc pyro_time)   ; only sense pyros every other time
        (ldaa pyro_time)
        (cmpa ! #x02)
        (bne do_adj_time)
        (clr pyro_time)

        (ldaa head_pyr)   ; read head pyro value
        (tab)             ; copy the value into b
        (suba last_head)  ; get the difference
        (stab last_head)  ; save current value as the new prev.
        (tsta)            ; see if it is negative
        (bge head_pos)
        (nega)            ; switch sign off diff if negative
        
     head_pos
        (cmpa ! #x05)     ; if difference <= 5 then no head sense
        (bls read_arm)
        (ldaa ! #x01)     ; set head flag
        (staa head_fl)
        (ldaa servo_mode)  ; remember the servo mode
        (staa person_mode)

    read_arm
        (ldaa arm_pyr)    ; read head pyro value
        (tab)             ; copy the value into b
        (suba last_arm)   ; get the difference
        (stab last_arm)   ; save current value as the new prev.
        (tsta)            ; see if it is negative
        (bge arm_pos)
        (nega)            ; switch sign off diff if negative
        
     arm_pos
        (cmpa ! #x05)     ; if difference <= 5 then no arm sense
        (bls chk_1st)
        (ldaa ! #x01)     ; set arm flag
        (staa arm_fl)

    chk_1st
        (tst first_fl)   ; if first read then clear the flags
        (beq do_adj_time)
        (clr first_fl)
        (clr head_fl)
        (clr arm_fl)

    do_adj_time
        (tst adj_fl)      ; only adjust if flag on
        (beq time_done)
        (tst adj_time)    ; only dec if not zero
        (bmi time_done)
        (dec adj_time)


    time_done
        (rts)


; routine to handle different motion modes
     mode_handle
         (ldaa mot_mode)
         (cmpa ! #x01)         ; mode one, turn left?
         (bne mode2)
         (ldaa mot_cnt)       ; read motor state
         (bne no_st0a)
         (jsr turn_left)      ; state zero
         (rts)
     no_st0a
         (cmpa ! #x01)       
         (bne no_st1a)
         (ldaa last_cnt)       ; if last_cnt zero and no collisions, can exit
         (bne not_1sta)
         (ldaa ir_val)
         (beq clr_mode)
     not_1sta
         (tst hit_b)           ; if hit in the back then exit
         (bne clr_mode)
         (jsr turn_right)      ; state one
         (rts)
     no_st1a
         (cmpa ! #x02)       
         (bne no_st2a)
         (tst hit_b)           ; if hit in the back then exit
         (bne clr_mode)
         (jsr move_back)      ; state two
         (rts)
     no_st2a
         (cmpa ! #x03)       
         (bne clr_mode)
         (tst hit_b)           ; if hit in the back then exit
         (bne clr_mode)
         (jsr turn_left)     ; state three
         (rts)

     clr_mode
         (clr mot_time)      ; go back to regular mode
         (clr mot_cnt)
         (clr mot_mode)
         (rts)

     mode2
         (ldaa mot_mode)
         (cmpa ! #x02)        ; mode two, turn right?
         (bne mode3)
         (ldaa mot_cnt)       ; read motor state
         (bne no_st0b)
         (jsr turn_right)     ; state zero
         (rts)
     no_st0b
         (cmpa ! #x01)       
         (bne no_st1b)
         (ldaa last_cnt)       ; if last_cnt zero and no collisions, can exit
         (bne not_1stb)
         (ldaa ir_val)
         (beq clr_mode)
      not_1stb
         (tst hit_b)           ; if hit in the back then exit
         (bne clr_mode)
         (jsr turn_left)      ; state one
         (rts)
     no_st1b
         (cmpa ! #x02)       
         (bne no_st2b)
         (tst hit_b)           ; if hit in the back then exit
         (bne clr_mode)
         (jsr move_back)      ; state two
         (rts)
     no_st2b
         (cmpa ! #x03)       
         (bne clr_mode)
         (tst hit_b)           ; if hit in the back then exit
         (bne clr_mode)
         (jsr turn_right)     ; state three
         (rts)

     mode3
         (cmpa ! #x03)        ; mode three, stop?
         (bne mode4)
         (ldaa mot_cnt)       ; read motor state
         (bne clr_mode)
         (jsr stop_move)      ; state zero
         (rts)

     mode4
         (cmpa ! #x04)        ; mode four, back up
         (bne mode5)
         (ldaa mot_cnt)       ; read motor state
         (bne clr_mode)
         (jsr move_back)      ; state zero
         (rts)

     mode5
         (cmpa ! #x05)        ; mode five, go forward
         (bne mode6)
         (ldaa mot_cnt)       ; read motor state
         (bne clr_mode)
         (jsr move_for)      ; state zero
         (rts)

     mode6
         (cmpa ! #x06)        ; mode six, turn left
         (bne mode7)
         (ldaa mot_cnt)       ; read motor state
         (bne clr_mode)
         (jsr turn_left)      ; state zero
         (rts)

     mode7
         ; mode seven, turn right
         (bne mode6)
         (ldaa mot_cnt)       ; read motor state
         (bne go_clr_mode)
         (jsr turn_right)      ; state zero
         (rts)

     go_clr_mode
         (jmp clr_mode)



; routine to keep the arm in travel position
     arm_maint
         (jsr lim_read)   ; read the limit switches
         (bitb ! #x02)    ; is the wrist up?
         (bne fix_arm)

         (jsr stop_all)   ; stop motion
         (jsr wrist_up)

     fix_wrist  
         (jsr lim_read)    ; raise wrist until limit
         (bitb ! #x02)
         (beq fix_wrist)
         (jsr stop_wrist)

     fix_arm
         (jsr stop_wrist)  ; make sure wrist is stopped
         (jsr lim_read)    ; lower arm until limit
         (bitb ! #x01)     ; check if arm is down
         (beq fix_pos)
         (jsr stop_arm)
         (rts)

     fix_pos
         (jsr arm_dwn)
         (rts)

         
         
      
; variables used by the program
     (=v mot_state #x00)  ; this is the current state of the motor register
     (=v eye_state #x01)  ; this is the current state of the eye register
     (=v hit_l     #x02)  ; 1 if object on the left
     (=v hit_r     #x03)  ; 1 if object on the right
     (=v hit_f     #x04)  ; 1 if object on the front
     (=v hit_b     #x05)  ; 1 if object on the back
     (=v no_fl     #x06)  ; 1 if the floor is missing
                          ; 1 means active, bits: 0=turn
     (=v mot_time  #x07)  ; timer used for movement, switches
     (=v mot_cnt   #x08)  ; motor direction state
     (=v mot_mode  #x09)  ; mode of motion to perform
                          ;   0 = regular
                          ;   1 = turn left
                          ;   2 = turn right
                          ;   3 = stop for one count
                          ;   4 = back up for one count
                          ;   5 = go forward for one count
     (=v last_mode #x0a)  ; keeps track if last turn was left or right
     (=v last_lr   #x0b)  ; "" track if last L^R blocked move was B or F
     (=v last_cnt  #x0c)  ; keeps track of previous count value
     (=v ir_val    #x0d)  ; value read from the ir sensors
     (=v lim_val   #x0e)  ; value read from the limit switches

     (=v scan_time  #x0f) ; used to generate ~ .1 second ticks
     (=v servo_time #x10) ; used for timing a servo scan
     (=v servo_mode #x11) ; used to keep track of position in scan

     (=v head_fl     #x12) ; flag to tell if head pyro has sensed something
     (=v arm_fl      #x13) ; flag to tell if arm pyro has sensed something
     (=v first_fl    #x14) ; flags tells if this is the first pyro reading
     (=v last_head   #x15) ; this is the last head reading
     (=v last_arm    #x16) ; this is the last arm reading
     (=v person_pos  #x17) ; keeps track of person left/right position
     (=v start_mode  #x18) ; keeps track of servo_mode arm scan started in
     (=v person_mode #x19) ; keeps track of place target first seen
     (=v pyro_time   #x1a) ; used to keep track of how often to read the pyros
     (=v adj_fl      #x1b) ; used to tell when to dec adj_time
     (=v adj_time    #x1c) ; controls length of wrist adjustment
     (=v off_time    #x1d) ; time of supression of shoot behavior
     (=v search_time #x1e) ; time out and misc time for scan/shoot behavior
     (=v arm_dir     #x1f) ; which direction to adj arm in scan 0=up, 1=dwn
     (=v dont_shoot  #x20) ; flag to supress shoot behavior
     (=v no_switch   #x21) ; flag to supress switching behavior in shoot sweep
     (=v swing_length #x22); holds length of swing in tenth's of a second

            
         ))