.nolist
;ToDo's
;
;
.include "c:\avrtools\appnotes\8535def.inc"
.list

;------ PortA Defines ----------------------------------------
.equ    LeftPhoto       = 0     ;Left Photo sensor
.equ    RightPhoto      = 1     ;Right Photo sensor
.equ    RightRange      = 2     ;Right Range sensor
.equ    LeftRange       = 3     ;Left Range sensor
.equ    Battery         = 4     ;Battery monitor

;------ PortB Defines ----------------------------------------
.equ    btnMode         = 0     ;Mode button
.equ    btnRun          = 2     ;Run button

;------ PortC Defines ----------------------------------------

;------ PortD Defines ----------------------------------------
.equ    RedLed          = 2     ;red LED
.equ    GreenLed        = 3     ;green LED
.equ    LeftServo       = 4     ;left drive servo
.equ    RightServo      = 5     ;right drive servo
.equ    LeftLed         = 6     ;left floor LED
.equ    RightLed        = 7     ;right floor LED

;------ Macros for Port control ------------------------------
.macro  EnableFloorLED
                cbi portd,LeftLED
                cbi portd,RightLED
.endm
.macro  DisableFloorLED
                sbi portd,LeftLED
                sbi portd,RightLED
.endm
.macro  GreenLED
                sbi portd,GreenLed
                cbi portd,RedLed
.endm
.macro  RedLED
                cbi portd,GreenLed
                sbi portd,RedLed
.endm
.macro  LEDoff
                sbi portd,GreenLed
                sbi portd,RedLed
.endm

;------ Flag Bits ---------------------------------------
.equ    Tick            = 0     ;Timer Tick bit
.equ    Running         = 1     ;Running mode
.equ    BlinkOn         = 2     ;controls blink LED

;------ Mode Constants ----------------------------------
.equ    mdSearch        = 0     ;use eyes and search   Green      
.equ    mdAgressive     = 1     ;i have no idea        Red        
.equ    mdBlind         = 2     ;no eyes, just wander  Green blink
.equ    mdDiag          = 3     ;diagnostic mode       Red blink  
.equ    mdLastMode      = 4     ;end of table

;------ Sensor bits -------------------------------------
.equ    sRightFloor     = 0     ;Right floor sensor
.equ    sLeftFloor      = 1     ;Left floor sensor
.equ    sRightEye       = 2     ;Right eye sensor
.equ    sLeftEye        = 3     ;Left eye sensor

;------ Sensor Thresholds -------------------------------
.equ    Threshholdrf    = 0x80  ;Right floor
.equ    Threshholdlf    = 0x80  ;Left floor
.equ    Threshholdre    = 0x40  ;Right eye
.equ    Threshholdle    = 0x40  ;Left eye

;------ Register Defines --------------------------------
.def    ISRTimer0 =     r1      ;used in timer0 ISR
.def    ISRTimer2 =     r2      ;used in timer2 ISR
.def    ISRDelay =      r3      ;used in Delay routine
.def    UartTemp =      r4      ;used in Uart ISR
.def    ISRADC =        r5      ;used in ADC ISR
.def    ADClo =         r6      ;temp register used in ADC ISR
.def    ADChi =         r7      ;temp register used in ADC ISR

.def    Acc =           r16     ;used as accumulator
.def    Mode =          r17     ;running mode
.def    Flags =         r18     ;Flag bits
.def    LeftScan =      r19     ;scan value of left scanner  ???
.def    RightScan =     r20     ;scan value of right scanner ???
.def    MyLoop =        r21     ;loop counter
.def    BlinkCounter =  r22     ;controls LED blink rate
.def    Sensors =       r23     ;bit weighted sensor readings

;       x register      r26/r27 ;X register used by diag routine
.def    ADCchannel =    r28     ;Y register points to the current
.def    ADCchannelHi =  r29     ;ADC channel being measured

;------ Ram Defines -------------------------------------
.equ    sbBOB           = 0x60  ;begining of status block
.equ    sbRightPhoto    = 0x60  ;Right floor sensor
.equ    sbLeftPhoto     = 0x61  ;Left floor sensor
.equ    sbRightRange    = 0x62  ;Right Range sensor
.equ    sbLeftRange     = 0x63  ;Left Range sensor
.equ    sbBattery       = 0x64  ;Battery Voltage
.equ    sbEOB           = 0x65  ;end of status block

;------ Interrupt Vector Table --------------------------
        .cseg                   ;start code segment
        .org 0
        rjmp Start              ;reset vector
        reti                    ;irq0
        reti                    ;irq1
        reti                    ;timer2 compare
        rjmp Timer2             ;timer2 overflow
        reti                    ;timer1 capture
        reti                    ;timer1 compareA
        reti                    ;timer1 compareB
        reti                    ;timer1 overflow
        rjmp Timer0             ;timer0 overflow
        reti                    ;SPI transfer complete
        reti                    ;uart rx complete
        reti                    ;udr empty
        reti                    ;uart tx complete
        rjmp ADCIsr             ;adc complete
        reti                    ;eeprom ready
        reti                    ;analog compare

;****************************************************************
;       Timer0 - Timer Interrupt Service Routine                *
;                Entry occurs every 4ms                         *
;****************************************************************
Timer0:
        in      ISRTimer0,sreg          ;save status register
        sbr     Flags,1<<Tick           ;set tick flag
        out     sreg,ISRTimer0          ;restore status register
        reti

;****************************************************************
;       Timer2 - Timer Interrupt Service Routine                *
;                Entry occurs every 50ms.  This routine will    *
;                display the current mode on the bi-bolor LED   *
;                                                               *
;       Search          Green                                   *
;       Agressive       Red                                     *
;       Blind           Green blink                             *
;       Diag            Red blink                               *
;                                                               *
;****************************************************************
Timer2:
        in      ISRTimer2,sreg          ;save status register
        sbrc    Flags,Running           ;are we running?
        rjmp    T2end                   ;bail if we're running

        cpi     Mode,mdSearch
        breq    T2GreenOn
        cpi     Mode,mdAgressive
        breq    T2RedOn

        dec     BlinkCounter            ;blink rate counter
        brne    T2Toggle                ;jump if not time to change states

        ldi     BlinkCounter,4
        sbrs    Flags,BlinkOn           ;see which state the bit is in
        rjmp    T2SetBit
        cbr     Flags,1<<BlinkOn        ;clear the blink bit
        rjmp    T2Toggle
T2SetBit:
        sbr     Flags,1<<BlinkOn        ;set the blink bit

T2Toggle:
        cpi     Mode,mdBlind
        breq    T2Green                 ;keep the green on
        rjmp    T2Red                   ;keep the red on
T2Red:
        sbrs    Flags,BlinkOn
        rjmp    LedOff
T2RedOn:
        RedLed
        rjmp    T2end

T2Green:
        sbrs    Flags,BlinkOn
        rjmp    LedOff
T2GreenOn:        
        GreenLed
        rjmp    T2end

LedOff:
        LEDoff
T2end:
        out     sreg,ISRTimer2          ;restore status register
        reti

;****************************************************************
;       ACDIsr - Execute when the ADC conversion is complete.   *
;                First the 10 bits from the ADC are read into   *
;                registers.  Then the 2 LSB are dropped to form *
;                a single byte value.  This value is stored in  *
;                sram, based on the Y register with a post-inc. *
;                Then the lower bits of Y are written to the    *
;                ADC mux register and the next conversion starts*
;****************************************************************
ADCIsr:
        in      ISRADC,sreg             ;save status register
        in      ADClo,adcl              ;read the ADC
        in      ADChi,adch              ;read the ADC
        ror     ADChi                   ;put bit 8 into carry
        ror     ADClo                   ;dump bit 0, shift in bit 8
        ror     ADChi                   ;put bit 9 in carry
        ror     ADClo                   ;dump bit 1, shift in bit 9
        st      y+,ADClo                ;save it to sram
        cpi     yl,sbEOB                ;test for end of status block
        brne    ADCIsr1                 ;jump if in range
        ldi     yl,sbBOB                ;point to begining of status block
ADCIsr1:
        out     admux,yl                ;point to next mux channel
        sbi     adcsr,adsc              ;start the next ADC conversion
        out     sreg,ISRADC             ;restore status register
        reti

;****************************************************************
;       Start - Start of code                                   *
;****************************************************************
Start:
        ldi     r16,0x00                ;porta 7-0 are inputs
        out     ddra,r16
        ldi     r16,0x00                ;portb 7-0 are inputs
        out     ddrb,r16
        ldi     r16,0xff                ;portc 7-0 are outputs
        out     ddrc,r16                
        ldi     r16,0xff                ;portd 7-0 are outputs
        out     ddrd,r16                
                        
        ldi     r16,0xff
        out     portb,r16               ;activate pullups                        
        ldi     r16,0xff
        out     portc,r16
        ldi     r16,0xff
        out     portd,r16

        ldi     r16,low(RAMEND)         ;load the stack pointer
        out     SPL,r16
        ldi     r16,high(RAMEND)
        out     SPH,r16

        ldi     r16,0x03                ;timer0 uses ck/64
        out     tccr0,r16

        ldi     r16,0b11110001          ;setup the timers for pwm
        out     tccr1a,r16              ;8-bit pwm
        ldi     r16,0x03                ;CK / 64
        out     tccr1b,r16
        clr     r16
        out     ocr1al,r16
        out     ocr1bl,r16              ;make sure the drive motors are off

        ldi     r16,0x07                ;timer2 uses ck/1024
        out     tccr2,r16

        ldi     r16,0b10001101          ;enable ADC and interrupt
        out     adcsr,r16               ;prescale of 32

        clr     Flags                   ;turn off all flags
        clr     yh                      ;clear the upper register pointer
        ldi     ADCchannel,sbBOB        ;start at begining of status block
        out     admux,ADCchannel        ;start with channel 0

        ldi     r16,0b01000001          ;enable timer2 and timer0
        out     timsk,r16          
        sei                             ;enable interrupts

        sbi     adcsr,adsc              ;start the ADC
        ldi     Mode,mdBlind            ;start in blind mode
        ldi     BlinkCounter,4          ;start the blink counter

        ldi     r16,12
        out     ubrr,r16                ;set baudrate to 19.2K
        sbi     ucr,txen                ;enable the uart

MainLoop:
        sbis    pinb,btnMode            ;test mode button
        rjmp    ModeButtonPressed       ;jump if pressed
        sbic    pinb,btnRun             ;test run button
        rjmp    MainLoop                ;jump if button not pressed

        rcall   WaitOnButtonRelease
        sbr     Flags,1<<Running        ;set the run flag
        EnableFloorLED                  ;turn on the floor LED's

        cpi     Mode,mdSearch
        breq    hRunSearch              ;use eyes and search
        cpi     Mode,mdAgressive
        breq    hRunAgressive           ;i have no idea
        cpi     Mode,mdBlind
        breq    hRunBlind               ;use blind mode
        cpi     Mode,mdDiag
        breq    hRunDiag                ;run the diagnostic routines
        rjmp    MainLoop                ;should never get here but just incase

hRunSearch:
        rjmp    RunSearch
hRunAgressive:
        rjmp    RunAgressive
hRunBlind:      
        rjmp    RunBlind
hRunDiag:
        rjmp    RunDiag

ModeButtonPressed:
        rcall   WaitOnButtonRelease
        ldi     BlinkCounter,4          ;start the blink counter
        inc     Mode                    ;change modes
        cpi     Mode,mdLastMode         ;test for lastmode
        brne    ModeOk                  ;jump if ok
        clr     Mode                    ;start over with first mode
ModeOk:
        rjmp    MainLoop

WaitOnButtonRelease:
        ldi     r16,20
        rcall   Delay                   ;debounce for 80ms
WaitOnMode:
        sbis    pinb,btnMode            ;test mode button
        rjmp    WaitOnMode
        sbis    pinb,btnRun             ;test run button
        rjmp    WaitOnMode
        ldi     r16,10
        rcall   Delay                   ;debounce for 40ms
        ret

;****************************************************************
;       RunBlind - Run the ring without eyes                    *
;****************************************************************
RunBlind:
        LEDoff
        rcall   CheckBattery            ;check battery voltage
        rcall   StartupDelay            ;5 second delay
        rcall   Drop                    ;reverse the motors and bring the front down
        rcall   StartMotors             ;let's go
        ldi     r16,50
        rcall   Delay                   ;wait 20ms for edge sensors to stablize
RB1:
        clr     Sensors
        lds     r16,sbRightPhoto        ;get the right floor sensor value
        cpi     r16,Threshholdrf        ;do we see the light?
        brsh    RB2                     ;jump if no light
        sbr     Sensors,1<<sRightFloor     ;indicate we saw the light
RB2:
        lds     r16,sbLeftPhoto         ;get the right floor sensor value
        cpi     r16,Threshholdlf        ;do we see the light?
        brsh    RB3                     ;jump if no light
        sbr     Sensors,1<<sLeftFloor      ;indicate we saw the light
RB3:
        cpi     Sensors,0x01            ;is just the right sensor on?
        brne    RB4                     
        rcall   GoLeft                  ;turn away from edge
        rjmp    RB1
RB4:        
        cpi     Sensors,0x02            ;is just the left sensor on?
        brne    RB5
        rcall   GoRight                 ;turn away from edge
        rjmp    RB1
RB5:        
        cpi     Sensors,0x03            ;are both sensors on?
        brne    RB1
        rcall   GoBack                  ;backup and turn
        rjmp    RB1                     ;keep having fun

GoLeft:
        rcall   LeftTurn
        ldi     r16,150
        rcall   Delay 
        rcall   StartMotors
        ret

GoRight:
        rcall   RightTurn
        ldi     r16,150
        rcall   Delay 
        rcall   StartMotors
        ret

GoBack:
        rcall   ReverseMotors
        ldi     r16,150
        rcall   Delay
        rjmp    GoLeft

;****************************************************************
;       RunSearch - Spin slowly around looking for opponent and *
;                   then go after it.                           *
;****************************************************************
RunSearch:
        LEDoff
        rcall   CheckBattery            ;check battery voltage
        rcall   StartupDelay            ;5 second delay
        rcall   Drop                    ;reverse the motors and bring the front down
        rcall   StartMotors             ;let's go
        ldi     r16,50
        rcall   Delay                   ;wait 20ms for edge sensors to stablize
        
SlowSearch:        
        rcall   SlowLeft                ;start a slow spin

RS1:
        clr     Sensors
        lds     r16,sbRightPhoto        ;get the right floor sensor value
        cpi     r16,Threshholdrf        ;do we see the light?
        brsh    RS2                     ;jump if no light
        sbr     Sensors,1<<sRightFloor  ;indicate we saw the light
RS2:
        lds     r16,sbLeftPhoto         ;get the right floor sensor value
        cpi     r16,Threshholdlf        ;do we see the light?
        brsh    RS3                     ;jump if no light
        sbr     Sensors,1<<sLeftFloor   ;indicate we saw the light
RS3:
        cpi     Sensors,0x01            ;is just the right sensor on?
        brne    RS4
        rcall   GoLeft                  ;turn away from edge
        rjmp    SlowSearch
RS4:
        cpi     Sensors,0x02            ;is just the left sensor on?
        brne    RS5
        rcall   GoBack
;        rcall   GoRight                 ;turn away from edge
        rjmp    SlowSearch
RS5:        
        cpi     Sensors,0x03            ;are both sensors on?
        brne    RS6                     ;check the eyes
        rcall   GoBack                  ;backup and turn
        rjmp    SlowSearch              ;keep having fun

RS6:    
        clr     Sensors                 ;start with no sensors
        lds     r16,sbRightRange        ;get the right eye
        cpi     r16,ThreshHoldRe        ;did we see something
        brlo    RS7                     ;jump if nothing seen
        sbr     Sensors,1<<sRightEye    ;set the sensor bit
RS7:        
        lds     r16,sbLeftRange         ;get the left eye
        cpi     r16,ThreshHoldLe        ;did we see something
        brlo    RS8                     ;jump if nothing seen
        sbr     Sensors,1<<sLeftEye     ;set the sensor bit
RS8:        
        cpi     Sensors,0x04            ;is something to the left?
        breq    RS10                    ;jump if something on the left
        cpi     Sensors,0x08            ;is something to the right?
        breq    RS11                    ;jump if something on the right
        cpi     Sensors,0x0c            ;is something in front of us?
        breq    RS12                    ;jump if there is
        rjmp    SlowSearch              ;keep having fun

RS10:     
        rcall   StartMotors             ;go forward a little
        ldi     r16,25
        rcall   delay
        rcall   LeftTurn                ;turn towards the object
        rjmp    RS1
RS11:
        rcall   StartMotors             ;go forward a little
        ldi     r16,25
        rcall   delay
        rcall   RightTurn                ;turn towards the object
        rjmp    RS1
RS12:
        rcall   StartMotors              ;go after it
        rjmp    RS1
        

;****************************************************************
;       RunAgressive - Run full speed looking for opponent and  *
;                      then go after it.                        *
;****************************************************************
RunAgressive:
        rjmp    MainLoop

;****************************************************************
;       RunDiag - Send the status block out the uart            *
;****************************************************************
RunDiag:
        lds     r16,sbLeftRange        ;get the right eye
;        cpi     r16,ThreshHoldRe        ;did we see something
        cpi     r16,0x40
        brlo    RD7                     ;jump if nothing seen
        GreenLED
        rjmp    RunDiag
RD7:
        RedLED
        rjmp    RunDiag        





        ldi     xl,low(sbBOB)           ;point to status block
        ldi     xh,high(sbBOB)
        ldi     r16,0xee
        rcall   SendByte                ;send header
        ldi     r16,0xee
        rcall   SendByte
RD1:
        ld      r16,x+                  ;get status block data
        rcall   SendByte                ;send it to uart
        cpi     xl,sbEOB                ;see if we're at the end of the block
        brne    RD1                     ;continue looping
        ldi     r16,10
        rcall   delay                   ;wait 40ms
        rjmp    RunDiag                 ;point back to the begining of the block

;****************************************************************
;       CheckBattery - Read the battery voltage via the AtoD    *
;                       if it's getting low then light the Red  *
;                       LED.                                    *
;****************************************************************
CheckBattery:
        lds     r16,sbBattery           ;get the reading
        cpi     r16,0xC0                ;test for ~5 volts
        brsh    CBend                   ;jump if battery ok
        RedLED                          ;light the LED
CBend:
        ret        
        
;****************************************************************
;       Motor Command Routines                                  *
;****************************************************************
StartMotors:
        ldi     r16,0xdf                ;0xdf = 1ms, 0xd0 = 1.5ms, 0xc0 = 2ms
        out     ocr1al,r16
        ldi     r16,0xc0                ;0xdf = 1ms, 0xd0 = 1.5ms, 0xc0 = 2ms
        out     ocr1bl,r16
        ret

ReverseMotors:
        ldi     r16,0xc0
        out     ocr1al,r16
        ldi     r16,0xdf
        out     ocr1bl,r16              ;put motors in reverse
        ret

StopMotors:
        clr     r16
        out     ocr1al,r16              ;turn off the motors
        out     ocr1bl,r16
        ret

LeftTurn:
        ldi     r16,0xc0
        out     ocr1al,r16
        ldi     r16,0xc0
        out     ocr1bl,r16              ;put motor in reverse
        ret

RightTurn:
        ldi     r16,0xdf
        out     ocr1al,r16
        ldi     r16,0xdf
        out     ocr1bl,r16              ;put motor in reverse
        ret

SlowLeft:
        ldi     r16,0xcc
        out     ocr1al,r16
        ldi     r16,0xcc
        out     ocr1bl,r16              
        ret

;****************************************************************
;       StartupDelay - Waits 5 seconds                          *
;****************************************************************
StartupDelay:
        ldi     myloop,5
SD1:
        EnableFloorLED          ;turn on the LED's
        ldi     r16,200
        rcall   delay           ;delay for 800ms

        DisableFloorLED         ;turn off the LED's
        ldi     r16,50
        rcall   delay           ;delay for 200ms

        dec     myloop
        brne    SD1             ;loop till 5 seconds are up

        EnableFloorLED          ;turn on the LED's
        ret

;****************************************************************
;       Drop - Reverse the motors and bring down the nose       *
;****************************************************************
Drop:
        rcall   ReverseMotors
        ldi     r16,80
        rcall   delay           ;let's try 320ms
        rcall   StopMotors
        ret

;****************************************************************
;       Delay - Delays R16 * 4 milliseconds                     *
;****************************************************************
Delay:  clr     ISRDelay
        out     tcnt0,ISRDelay          ;clear timer register
        cbr     Flags,1<<Tick           ;reset the tick flag
Delay1:
        sbrs    Flags,Tick              ;wait till timer interrupt sets Tick flag
        rjmp    Delay1                  ;loop till interrupt occurs

        cbr     Flags,1<<Tick           ;reset the tick flag
        dec     r16                     ;decrement counter
        brne    Delay1                  ;loop until counter = 0
        ret

;****************************************************************
;       ShortDelay - Waits 100us or 400 cycles
;****************************************************************
ShortDelay:                     ;3
        push    r16             ;2
        ldi     r16,130         ;1
SDY:                            ;       6
        dec     r16             ;1
        brne    sd1             ;2
                                ;       395
        pop     r16             ;2
        ret                     ;4
                                ;       401

;****************************************************************
;       SendByte - The contents of R16 are sent to the UART     *
;****************************************************************
SendByte:
        sbis    usr,udre                ;check the UART status
        rjmp    SendByte                ;loop till UART ready
        out     udr,r16                 ;send the command
        ret