/*
lightseek.Asm

Copyright ©1998, 1999 Larry Barello

Author: Larry Barello
        lbarello@accessone.com

You are allowed to use this file for personal use only.

You may use, embed, modify or distribute the contents
of this file in any way you see fit as long as you 
include this copyright notice in the resulting file.

Any commercial or industrial use including publishing
or distributing in any form requires the written 
authorization of the copyright holder(s).

No guarantees are made about fitness, functionality, 
correctness or performance of this file.  The file is 
supplied on a "AS IS" basis, with no responsibility 
assumed by the author(s) for bug fixes, support, or 
damages of any sort arising from the use of this file. 

Contact the author with problem reports or fixes, 
feature requests or additions and questions.  They 
are welcomed and will be incorporated as time and 
energy permits.
*/
#include        "Registers.inc"
#include        "avrx.inc"

        NAME    LIGHTSEEK

        EXTERN  TICKRATE RoboSpin AvrXReadEEProm LightCycleTime
        EXTERN  AvrXDelay AvrXStartTimer AvrXWaitTimer
        EXTERN  Left_Eye Right_Eye AvrXBreakpoint
        
        PUBLIC  LightSeekTcb LightSeekPid

        RSEG    SSEG:DATA(1)
;++++++++++++++++++++++++++++++++++++++++++++++
; Stack = Task Usage (2) + 2 stacked interrupts + Context Save
; plus any used by the system in normal tasking (Interrupt nesting (3), System
; call (0-3 bytes))  AVR stack points to first save location (post dec, pre inc)
; so need one additional byte at location of lable.
;
                DS      10      ; Standard Context
                DS      4       ; User stack
                DS      1       ; plus one interrupt
LightSeekStack: DS      1

        RSEG    AVRXDATA:DATA(0)
        
LightSeekPid:   DS      PidSz
LightSeekTimer: DS      TcbSz


        RSEG    CODE

;----------------------------------------------
;
LightSeekTcb:
        dw      LightSeekStack  ; Stack
        dw      LightSeek>>1    ; Start Address
        db      LightSeekPid, 0
        dw      0
        db      5               ; Priority

        EVEN
;**************************************************************************
;
; LightSeek
;
; Periodically check the eye's and spin the platform towards the 
; stronger eye
;
; NOTES:
;
LightSeek:
        ldi     Zl, low(LightCycleTime)
        ldi     Zh, High(LightCycleTime)
        rcall   AvrXReadEEProm
        mov     Yh, Yl
        adiw    Zl, 1                   ; Timer value loaded from EEProm
        rcall   AvrXReadEEProm

        ldi     Xl, low(LightSeekTimer)
        rcall   AvrXStartTimer
        rcall   AvrXWaitTimer

        lds     Yl, Left_Eye
        lds     Xl, Right_Eye
        clr     Yh
        sub     Yl, Xl
        sbci    Yh, 0
        rcall   RoboSpin

        rjmp    LightSeek

        END