;       FlatSpat Firmware

.nolist
.include "c:\avrtools\appnotes\8535def.inc"
.list

;------ PortA Defines ----------------------------------------
.equ    RightFloor      = 1     ;Right floor sensor
.equ    LeftFloor       = 0     ;Left floor sensor

;------ PortB Defines ----------------------------------------
.equ    LeftRange       = 0     ;Left Range sensor
.equ    RightRange      = 1     ;Right Range sensor
.equ    GreenLed        = 2     ;green LED
.equ    btnRun          = 3     ;Run button
.equ    btnDiag         = 5     ;diag button
.equ    LeftDiagLED     = 6     ;left diag LED
.equ    RightDiagLED    = 7     ;right diag LED

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

;------ PortD Defines ----------------------------------------
.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  GreenLEDon
                cbi portb,GreenLed
.endm
.macro  GreenLEDoff
                sbi portb,GreenLed
.endm
.macro  LeftLEDon
                cbi portb,LeftDiagLED
.endm
.macro  LeftLEDoff
                sbi portb,LeftDiagLED
.endm
.macro  RightLEDon
                cbi portb,RightDiagLED
.endm
.macro  RightLEDoff
                sbi portb,RightDiagLED
.endm

;------ Flag Bits ---------------------------------------
.equ    Tick            = 0     ;Timer Tick bit
.equ    Running         = 1     ;Running mode
.equ    BlinkOn         = 2     ;controls blink LED
.equ    sInFront        = 3     ;Object is in front of us
.equ    sLeft           = 4     ;Object is to the left
.equ    sRight          = 5     ;Object is to the right

;------ Mode Constants ----------------------------------
.equ    mdSearch        = 0     ;use eyes and search   Green
.equ    mdBlind         = 1     ;no eyes, just wander  Green blink

;------ 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

;------ Register Defines --------------------------------
.def    ISRTimer0 =     r1      ;used in timer0 ISR
.def    ISRTimer2 =     r2      ;used in timer2 ISR
.def    ISRDelay =      r3      ;used in Delay routine

.def    Acc =           r16     ;used as accumulator
.def    Mode =          r17     ;current mode
.def    Flags =         r18     ;Flag bits
.def    MyLoop =        r19     ;loop counter
.def    BlinkRate =     r20     ;controls LED blink rate
.def    Sensors =       r21     ;bit weighted sensor readings
.def    temp =          r22     ;temp used in BlinkValue
.def    SearchCounter = r23     ;used during search routines

;------ 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
        reti                    ;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 LED            *
;                                                               *
;       Search          Green                                   *
;       Blind           Green 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    T2SetBit                ;jump if Search mode

        dec     BlinkRate               ;blink rate counter
        brne    T2end                   ;jump if not time to change states

        ldi     BlinkRate,4
        sbrs    Flags,BlinkOn           ;see which state the bit is in
        rjmp    T2SetBit
T2ClrBit:
        cbr     Flags,1<<BlinkOn        ;clear the blink bit
        GreenLedOff                     ;turn off the LED
        rjmp    T2end
T2SetBit:
        sbr     Flags,1<<BlinkOn        ;set the blink bit
        GreenLedON                      ;turn on the LED
T2end:
        inc     SearchCounter           ;used during searches
        out     sreg,ISRTimer2          ;restore status register
        reti

;****************************************************************
;       Start - Start of code                                   *
;****************************************************************
Start:
        ldi     r16,0x00                ;porta 7-0 are inputs
        out     ddra,r16
        ldi     r16,0xf4                ;portb 3,1-0 are inputs
        out     ddrb,r16                ;portb 7-4,2 are output
        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     porta,r16               ;activate pullups
        out     portb,r16
        out     portc,r16
        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,0b01000001          ;enable timer2 and timer0
        out     timsk,r16
        sei                             ;enable interrupts

        clr     Flags                   ;turn off all flags
        ldi     Mode,mdSearch           ;start in search mode
        ldi     BlinkRate,4             ;start the blink counter

;       First see if the diag button is pressed

        sbis    pinb,btnDiag            ;test diag button
        rjmp    RunDiag                 ;run the diag routine

;       Now see if the run button is pressed at powerup
;       switch to blind mode if pressed

        sbic    pinb,btnRun             ;test run button
        rjmp    MainLoop                ;jump if using search mode

        ldi     Mode,mdBlind            ;start in blind mode
        rcall   WaitOnButtonRelease     ;wait till button released

MainLoop:
        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,mdBlind
        breq    hRunBlind               ;use blind mode
        rjmp    MainLoop                ;should never get here but just incase

hRunSearch:
        rjmp    RunSearch
hRunBlind:
        rjmp    RunBlind

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

;****************************************************************
;       RunBlind - Run the ring without eyes                    *
;****************************************************************
RunBlind:
        GreenLEDoff
        rcall   StartupDelay            ;5 second delay
        rcall   Drop                    ;reverse the motors and bring the front down
        rcall   StartMotors             ;let's go
        ldi     r16,100
        rcall   Delay                   ;wait 400ms for edge sensors to stablize
RB1:
        clr     Sensors
        sbic    pina,RightFloor         ;test the right floor sensor
        sbr     Sensors,1<<sRightFloor  ;indicate we saw the light
RB2:
        sbic    pina,LeftFloor          ;test the left floor sensor
        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:
        GreenLEDoff
        rcall   StartupDelay            ;5 second delay
        rcall   Drop                    ;reverse the motors and bring the front down
        rcall   StartMotors             ;let's go
        ldi     r16,100
        rcall   Delay                   ;wait 400ms for edge sensors to stablize

SlowSearch:
        clr     SearchCounter           ;clear search timer
RS0:
        cbr     Flags,1<<sInFront       ;clear the flag
        rcall   SlowLeft                ;start a slow spin
RS1:
        clr     Sensors
        sbic    pina,RightFloor         ;test the right floor sensor
        sbr     Sensors,1<<sRightFloor  ;indicate we saw the light
RS2:
        sbic    pina,LeftFloor          ;test the left floor sensor
        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
        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:                                    ;here to check the eyes
        clr     Sensors                 ;start with no sensors
        sbic    pinb,RightRange         ;check right eye
        sbr     Sensors,1<<sRightEye    ;set the sensor bit
RS7:
        sbic    pinb,LeftRange          ;check left eye
        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
        sbrc    Flags,sInFront          ;was something in front of us?
        rjmp    RSInFront               ;jump if object was in front of us
        sbrc    Flags,sLeft             ;was something to the left?
        rjmp    RSLeft                  ;jump if object was to the left
        sbrc    Flags,sRight            ;was something to the right?
        rjmp    RSRight                 ;jump if object was to the right

        cpi     SearchCounter,100       ;5 second test
        brlo    RS0                     ;continue searching
        rcall   StartMotors             ;after 5 seconds of searching, move elsewhere
        rjmp    RS1                     ;now start searching

RSLeft:
        rcall   LeftTurn                ;turn towards the object
        rjmp    RS1

RSRight:
        rcall   RightTurn               ;turn towards the object
        rjmp    RS1

RSInFront:
        ;here if an object was in front of us and now isn't assume that the
        ;object is too close to the sensors and is giving false readings

        rcall   StartMotors             ;full speed ahead!!!
        rjmp    RS1                     ;watch the edge sensors

RS10:
        rcall   StartMotors             ;go forward a little
        ldi     r16,25
        rcall   delay
        rcall   LeftTurn                ;turn towards the object
        sbr     Flags,1<<sLeft          ;indicate the object is left of us
        cbr     Flags,1<<sRight
        cbr     Flags,1<<sInFront
        rjmp    RS1
RS11:
        rcall   StartMotors             ;go forward a little
        ldi     r16,25
        rcall   delay
        rcall   RightTurn               ;turn towards the object
        sbr     Flags,1<<sRight         ;indicate the object is right of us
        cbr     Flags,1<<sLeft
        cbr     Flags,1<<sInFront
        rjmp    RS1
RS12:
        rcall   StartMotors             ;go after it
        sbr     Flags,1<<sInFront       ;indicate we now have the object in front of us
        cbr     Flags,1<<sLeft
        cbr     Flags,1<<sRight
        rjmp    RS1

;****************************************************************
;       RunDiag - do something with the debug board             *
;****************************************************************
RunDiag:
        rjmp    diagEyes

;       test the floor sensors
rd0:
        EnableFloorLED
        sbis    pina,LeftFloor          ;test the left floor sensor
        rjmp    rd1                     ;jump if sensor sees light
        LeftLEDon
        rjmp    rd10
rd1:
        LeftLEDoff

rd10:
        sbis    pina,RightFloor         ;test the right floor sensor
        rjmp    rd11                    ;jump if sensor sees light
        RightLEDon
        rjmp    rd20
rd11:
        RightLEDoff

rd20:
        rjmp    rd0

diagEyes:
        sbis    pinb,LeftRange          ;test the left eye
        rjmp    rd21
        LeftLEDon
        rjmp    rd25
rd21:
        LeftLEDoff

rd25:
        sbis    pinb,RightRange         ;test the right eye
        rjmp    rd26
        RightLEDon
        rjmp    rd27
rd26:
        RightLEDoff
rd27:
        rjmp    diagEyes


;****************************************************************
;       BlinkValue - Display the value in r16 by blinking the   *
;                    green LED.                                 *
;****************************************************************
BlinkValue:
        mov     temp,r16                ;ok we're really using temp
        push    temp
        swap    temp                    ;use the upper nibble
        rcall   BlinkNibble             ;blink the upper nibble
        ldi     r16,80
        rcall   Delay                   ;wait 320ms between nibbles
        pop     temp                    ;recall data
        rcall   BlinkNibble             ;blink the lower nibble
        ret

BlinkNibble:
        andi    temp,0xf                ;mask off upper nibble
        breq    BNdone                  ;skip if zero
BN1:
        GreenLEDon
        ldi     r16,25
        rcall   Delay                   ;LED on for 100ms
        GreenLEDoff
        ldi     r16,25
        rcall   Delay                   ;LED on for 100ms
        dec     temp
        brne    BN1                     ;loop till done
BNdone:
        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,0xc0
        out     ocr1al,r16
        ldi     r16,0xc0
        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   StartMotors
        ldi     r16,30
        rcall   delay
        rcall   ReverseMotors
        ldi     r16,50
        rcall   delay
        rcall   StartMotors
        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