/*
File:   Sumobot.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"
#include        "hardware.inc"

        NAME  SUMOBOT
        
#ifndef NOMONITOR
        EXTERN  MonitorPid MonitorTcb MonitorEntry MonitorStack
#endif

#ifndef NOTARGETING
        EXTERN  TargetingPid TargetingTcb
#endif

#ifndef NONEARTARGETING
        EXTERN  NearTargetingPid NearTargetingTcb
#endif

        EXTERN  BoundryPid BoundryTcb DoMotorDrive
        
        EXTERN  AvrXRunTask AvrXInitTask AvrXSetSemaphore
        EXTERN  AvrXResume AvrXSuspend Clear_Ram Clear_Registers
        EXTERN  _IntProlog _Epilog AvrXTimerHandler EEPromMutex
#ifndef NOMONITOR
        EXTERN  _GetChar PutChar RxC_Interrupt _PrintString PushChar
        EXTERN  PutHexWord PutHexByte GetHexWord GetHexByte
        EXTERN  PutCR PutSpace IsWhite InitSerialIO UDRE_Interrupt
#endif
        PUBLIC  TICKRATE CPUCLK AvrXStack
        PUBLIC  TCNT0_INIT
;
; High Speed Counter settings:  150khz interrupt rate: ~600 chop rate
;
TCCR0_INIT      = CLK64
TCNT0_INIT      = 255           ; Tick over every 64 cycles
;
; This file contains the hardware definitions and the code to
; boot the MPU after a harware reset.
;
; Hardware Equates
;
CPUCLK          = 11059200 
TICKRATE        = 2400          ; 417us system tick
TCCR1A_INIT     = 0
TCCR1B_INIT     = CLK256        ; big enough to avoid fancy math
                                ; to account for interrupt latency
TCNT1_INIT      = 65536-(CPUCLK/256)/TICKRATE

AvrXStack:      =       RAMEND

;------------------------------------------------
; Interrupt Vector Table
;
        RSEG    INTVECT

        ORG     0
        rjmp    CPU_Reset
        ORG     INT0addr
        ORG     INT1addr
        ORG     ICP1addr
        ORG     OC1Aaddr 
        ORG     OVF1addr
        rjmp    OVF1_Interrupt          ; System Clock
        ORG     OVF0addr
        rjmp    DoMotorDrive            ; Sumo.asm
#ifndef NOMONITOR
        ORG     URXCaddr
        rjmp    RxC_Interrupt
        ORG     UDREaddr
        rjmp    UDRE_Interrupt
        ORG     UTXCaddr
#endif
        ORG     ACIaddr

        RSEG    CODE
;+        
;----------------------------------------------
; OVF0_Interrupt
;
; Reload counter then jmp to AvrX_TimerInterrupt
; every millisecond.
;
; NOTES:        Reload timer before anything else since
;               interrupts are enabled after an IntProlog and
;               subsequent actions might be delayed long enough
;               for a timer overflow to occur (and miss a tick)
;-
OVF1_Interrupt:
        BeginInterrupt
        rcall   _IntProlog
        
        ldi     R30, low (TCNT1_INIT)   ; Reload timer
        out     TCNT1L, R30
        ldi     R30, high (TCNT1_INIT)  ; Reload timer
        out     TCNT1H, R30

        rcall   AvrXTimerHandler

        rjmp    _Epilog
;+
;-----------------------------------------------
; CPU_Reset
;
; Does whatever 
;
;-
CPU_Reset:
        ldi     Zl, low(RAMEND) ; System stack at end of RAM (small)
        ldi     Zh, high(RAMEND)
        out     SPl, Zl
        out     SPh, Zh

        rcall   Clear_Ram
        rcall   Clear_Registers

        INIT_DEBUG                      ; Macro from AvrX.inc
#ifndef NOMONITOR        
        ldi     Zl, low(MonitorTcb)
        ldi     Zh, high(MonitorTcb)
        rcall   AvrXRunTask
        rcall   InitSerialIO            ; Initialize user i/f
#endif
#ifndef NOTARGETING
        ldi     Zl, low(TargetingTcb)   ; Target seek process
        ldi     Zh, high(TargetingTcb)
        rcall   AvrXRunTask
#endif

#ifndef NONEARTARGETING
        ldi     Zl, low(NearTargetingTcb)   ; Near Target seek process
        ldi     Zh, high(NearTargetingTcb)
        rcall   AvrXRunTask
#endif

        ldi     Zl, low(BoundryTcb)     ; Boundry avoidance Process
        ldi     Zh, high(BoundryTcb)
        rcall   AvrXRunTask
;
        ldi     Zl, MOTORDDR_INIT
        out     MOTOR-1, Zl             ; DDR
        sbi     MOTOR, Vin              ; Sharp Control Input

        ldi     Zl, SENSORPORT_INIT
        out     SENSOR+2, Zl            ; Enable pull up resistors
        ;
        ; Set Sleep mode
        ;
        ldi     Zl, 1<<SE
        out     MCUCR, Zl
        ;
        ; Timer 0 interrupt drives motor clock
        ;
        ldi     Zl, TCCR0_INIT
        out     TCCR0, Zl
        ldi     Zl, TCNT0_INIT
        out     TCNT0, Zl
        ;
        ; PWM (Timer 1) Overflow interrupt drives System clock
        ;
        ldi     Zl, TCCR1A_INIT
        out     TCCR1A, Zl
        ldi     Zl, TCCR1B_INIT
        out     TCCR1B, Zl
        ldi     Zl, high (TCNT1_INIT)   ; Must load high byte first (see spec)
        out     TCNT1H, Zl
        ldi     Zl, low (TCNT1_INIT)
        out     TCNT1L, Zl
        ;
        ; Enable Timer Overflow Interrupts
        ;
        ldi     Zl, 1<<TOIE0 + 1<<TOIE1
        out     TIMSK, Zl
        ;
        ; Prime the EEProm access Mutex
        ;
        ldi     Xl, low(EEPromMutex)    ; Prime the EEprom MUTEX
        rcall   AvrXSetSemaphore
        ;
        ; Schedule up our first task
        ;
        rjmp    _Epilog                 ; Run first task

        END