* Marek Handl
* April 25, 2007
* Milwaukee School of Engineering
* CS391 - Embedded Computer System Design
* 
* Light follower for HandyBoard Robot
* using light sensors (photo-resistors) connected to analog ports 2 and 3 (0 and 1 seem not to be working)
* connecting the sensors - one wire goes to the bottom line, the other one goes to the third line (from the bottom)
* analog ports return value 0-255, higher value means less light
* 
* 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

option = 0x1039     ; A/D setup
ADPU  = (1 << 7)    ; A/D setup
adctl = 0x1030      ; A/D control register
adr1 = 0x1031       ; result from analog port 0
adr2 = 0x1032       ; result from analog port 1
adr3 = 0x1033       ; result from analog port 2
adr4 = 0x1034       ; result from analog port 3


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

  .section    .data 
actual:     .word     0x0000                  ; store calculated voltage



.section .text

.global main
main:
  ; initialization
  clr   MOTORS
  clr   state
  clr   working  
  
  ; A/d initialization
  ldaa  option
  oraa  #ADPU       ; power up A/D system
  staa  option
  ldaa  #0b00110000 ; scan of four ports of PE4
  staa  adctl
   
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   work   ; 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
  
  
work:  
  ldaa  adr3    ; left sensor
  cmpa  adr4    ; compare it to right sensor value
  blo   goleft  ; branch if lower
  bgt   goright ; branch if greater
  
  ; same value - go straight
  ldaa  #bothon
  bra   9f
    
goleft:  
  ; going to the left
  ldaa  #righton
  bra   9f
  
goright:
  ; going to the right
  ldaa  #lefton
  
9:
  staa  MOTORS
  jsr   wait
  bra   loop
  
wait:
  ldx   #0xFFFF
1:
  dex
  bne   1b  
  rts












        
* ******************************************
        
        
        addb    adr1        ;add first result together
        addb    adr2        ;add second result
        adca    #0x00       ;add carry into upper byte of D
        addb    adr3        ;add third result
        adca    #0x00       ;add carry into upper byte of D
        addb    adr4        ;add final result
        adca    #0x00       ;add carry into upper byte of D
        addb    #0x02       ;add offset of 2
        adca    #0x00       ;add any carry to upper byte of D

        lsld                ;shift sum left six times     
        lsld
        lsld
        lsld
        lsld
        lsld

        ldx     #131          ;load x with 131

        idiv                ;divide sum by 131 to get result in x

        stx     actual      ;store voltage result

        rts











* *********************************************************************
* * obsolete


  
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