* Marek Handl
* April 25, 2007
* Milwaukee School of Engineering
* CS391 - Embedded Computer System Design
* 
* Line tracker for HandyBoard Robot
* robot follows a black line
* three sensors (black/white) need to be connected to digital inputs (15 - L, 14 - C, 13 - R)
* left motor needs to be connected to motor 3
* right motor needs to be connected to motor 2

; Sensors - white means 1 on digital input (black is zero)

* addresses of registers
PORTA   = 0x1000
PORTD   = 0x1008
DDRD    = 0x1009; Data Direction Register for Port D
TMSK1   = 0x1022
TFLG1   = 0x1023
TMSK2   = 0x1024
TFLG2   = 0x1025
PACTL   = 0x1026	; Pulse Accumulator Control Register
PACNT   = 0x1027
TCNT    = 0x100E	; Timer Count Register
TOC1    = 0x1016
TOC2    = 0x1018
TOC4    = 0x101C
SCCR1   = 0x102C	; Serial Communication Interface Control Register 1
SCCR2   = 0x102D	; Serial Communication Interface Control Register 2
SCSR    = 0x102E	; Serial Communication Interface Status Register
SCDR    = 0x102F	; Serial Communication Interface Data Register
BAUD    = 0x102B
SPCR    =	0x1028
MOTORS  = 0x7000  ; motors output
DIGIN   = 0x7FFF  ; Digital input

* constants
lefton  = 0x80    ; left motor on
righton = 0x40    ; right motor on
bothon  = 0xC0    ; left and right motor on

startbt = 0b10000000  ; start button, active in low
stopbt  = 0b01000000  ; stop button, active in low

.section .bss
; variables
state:   .rmb 1
working: .rmb 1   ; stopped...0, working...1

.section .text

.global main
main:
  ; initialization
  clr   MOTORS
  clr   state
  clr   working  
  
loop:
  ; *** check for pressed button ***
  ldaa  DIGIN
  ldab  working
  beq   stopped   
  ; working, check for stop button
  anda  #stopbt    ; if equals 0, button was pushed
  bne   linetracker   ; keep working, button not pushed
  staa  working
  beq   loop
stopped:
  ; stopped, check for start button
  anda  #startbt
  beq   started     ; button pushed
  clr   MOTORS
  bra   loop
started:
  ldaa  #1
  staa  working
  ldaa  #bothon
  staa  MOTORS
  
linetracker:  
  ; *** line tracker ***
  
  ldd   DIGIN    ; load to A from digital input
  anda  #0b00111000   ; bit5 = Left, bit4 = Center, bit3 = Right
  
  cmpa  #0b00111000    ; all white
  bne   left

  ; all white
  ldab  state         ; previous state of motors
  cmpb  #0b00101000      ; both motors on
  bne   1f
  ldab  #lefton      ; switch off the right motor
  bra   done
1:
  ldab  state           ; both motors off
  bne   2f
  ldab  #bothon     ; switch on both motors
2:
  ; one motor was on, keep moving in the same direction
  bra   done
  
left:
  ; check left sensor
  ldab  state
  anda  #0b00101000    ; ignore center sensor
  cmpa  #0             ; left and right both black ... keep moving in the same direction 
  ; TO BE DONE - if no motors going, switch both on
  beq   done

  clrb                ; clear B - temporary place for motor directions
  psha
  anda  #0b00100000    ; left sensor
  beq   right
  orab  #lefton         ; turn on left motor
  
right:
  ; check right sensor
  pula
  anda  #0b00001000    ; right white
  beq   done
  orab  #righton         ; turn on right motor
  
done:
  ; set up motors
  stab  MOTORS
  stab  state
  ; rts
  bra   loop          ; neverending loop