.equ __30F4011, 1
.include "p30f4011.inc"

;..............................................................................
;Configuration bits:
;..............................................................................

        config __FOSC, CSW_ON_FSCM_OFF & FRC & HS    ;Turn off clock switching and
                                            ;fail-safe clock monitoring and
                                            ;use the External Clock as the
                                            ;system clock

        config __FWDT, WDT_OFF              ;Turn off Watchdog Timer

        config __FBORPOR, PBOR_ON & BORV_27 & PWRT_16 & MCLR_EN
                                            ;Set Brown-out Reset voltage and
                                            ;and set Power-up Timer to 16msecs
                                            
        config __FGS, CODE_PROT_OFF         ;Set Code Protection Off for the 
                                            ;General Segment
;..............................................................................
;Program Specific Constants (literals used in code)
;..............................................................................

        .equ SAMPLES, 64         ;Number of samples


;..............................................................................
;Global Declarations:
;..............................................................................

        .global _wreg_init       ;Provide global scope to _wreg_init routine
                                 ;In order to call this routine from a C file,
                                 ;place "wreg_init" in an "extern" declaration
                                 ;in the C file.

        .global __reset          ;The label for the first line of code. 

        .global __T1Interrupt    ;Declare Timer 1 ISR name global

        .global __CNInterrupt    ;Declare Timer 1 ISR name global

;..............................................................................
;Constants stored in Program space
;..............................................................................

        .section .myconstbuffer, code
        .palign 2                ;Align next word stored in Program space to an
                                 ;address that is a multiple of 2
ps_coeff:
        .hword   0x0002, 0x0003, 0x0005, 0x000A




;..............................................................................
;Uninitialized variables in X-space in data memory
;..............................................................................

         .section .xbss, bss, xmemory
x_input: .space 2*SAMPLES        ;Allocating space (in bytes) to variable.
SPEED:   .space 2   ; CAN output signal

;..............................................................................
;Uninitialized variables in Y-space in data memory
;..............................................................................

          .section .ybss, bss, ymemory
y_input:  .space 2*SAMPLES



;..............................................................................
;Uninitialized variables in Near data memory (Lower 8Kb of RAM)
;..............................................................................

          .section .nbss, bss, near
var1:     .space 2               ;Example of allocating 1 word of space for
                                 ;variable "var1".
SUPER_VPN_FLAG:              .space 2   ; reserve 2 bytes, see description below
READY_CHECK_POSITION_FLAG:   .space 2
CW_FLAG:                     .space 2
CCW_FLAG:                    .space 2
UNDER_VPN_FLAG:              .space 2   ; HONEY_1 signal
SHUT_CURRENT_FLAG:           .space 2   ; HONEY_2 signal
PDC1_VAL:                    .space 2   ; PWM1 value
PDC2_VAL:                    .space 2   ; PWM2 value
VOER_VPN_FLAG:               .space 2   ; PWM1 value
OVER_CURRENT_FLAG:           .space 2   ; PWM2 value
FIRST_VPN_FLAG:              .space 2   ; CAN input signal
COUNT:                       .space 2   ; CAN output signal
OVER_TM1_VAL:                .space 2   ; Accelerate value
SPEED_COUNT:                 .space 2   ; CAN output signal


;..............................................................................
;Code Section in Program Memory
;..............................................................................

.text                             ;Start of Code section
__reset:
        MOV #__SP_init, W15       ;Initalize the Stack Pointer
        MOV #__SPLIM_init, W0     ;Initialize the Stack Pointer Limit Register
        MOV W0, SPLIM
        NOP                       ;Add NOP to follow SPLIM initialization
        
        CALL _wreg_init           ;Call _wreg_init subroutine
                                  ;Optionally use RCALL instead of CALL

; Setup CN control registers
        BSET TRISC,#13
        BSET TRISC,#14
        BSET CNEN1,#0
        BCLR IFS0,#CNIF
        MOV #0x1000,W0
        MOV,W0,IPC3
        BSET IEC0,#CNIE
        CLR READY_CHECK_POSITION_FLAG
        CLR CW_FLAG
        CLR CCW_FLAG
; Setup 32-bit timer control registers
        MOV #0x0030,W0
        MOV W0,T2CON
        MOV W0,T3CON
        CLR TMR3
        CLR TMR2
        MOV #0xFFFF,W0
        MOV W0,PR2
        MOV W0,PR3
        BSET T2CON,#T32
        BSET T2CON,#TON
        CLR SPEED_COUNT

MAIN_LOOP:

       NOP
       NOP 
       NOP
       NOP
       NOP
       NOP
       NOP 
       NOP
       NOP
       NOP
       NOP      
       BRA    MAIN_LOOP   
       
;*****************************************************************************
;
;
;           This is the CN0 Interrupt Service Routine
;
;
;*****************************************************************************
__CNInterrupt:
        PUSH.D W0
        PUSH.D W2   
        PUSH.D W4

        CALL CHECK_SPEED
        CP0 READY_CHECK_POSITION_FLAG
        BRA NZ, CHECK_POSISTION
        BTSC PORTC,#14
        BTSC PORTC,#13
        BRA END_HALT
        SETM READY_CHECK_POSITION_FLAG
        BRA END_HALT
CHECK_POSISTION:
        CLR CW_FLAG
        CLR CCW_FLAG
        BTSC PORTC,#13
        BRA DEFINE_CCW
        BRA DEFINE_CW
DEFINE_CW:
        SETM CW_FLAG
        CLR READY_CHECK_POSITION_FLAG
        BRA END_HALT
DEFINE_CCW:
        SETM CCW_FLAG
        CLR READY_CHECK_POSITION_FLAG
        BRA END_HALT
END_HALT: 
        BCLR IFS0, #CNIF    
        POP.D W4                      
        POP.D W2                   
        POP.D W0  
        RETFIE  

CHECK_SPEED:
        MOV SPEED_COUNT,W2
        MOV #0x0009,W4
        CP W2,W4
        BRA GE,CONFIRM_SPEED
        BRA LT,PLUS_SPEED
        RETURN

PLUS_SPEED:
        INC SPEED_COUNT
        RETURN

CONFIRM_SPEED:
        CLR SPEED_COUNT
        CP0 TMR3
        BRA NZ,CHECK_BIG
        BRA CHECK_SMALL
        RETURN

CHECK_BIG:
        MOV TMR3,W4
        MOV #0xEA60,W2
        REPEAT #17
        DIV.U W2,W4
        MOV W0,SPEED
        REPEAT #9
        LSR SPEED
        CLR TMR2
        CLR TMR3
        RETURN

CHECK_SMALL:
        MOV TMR2,W11
        MOV #0xEA60,W2
        REPEAT #17
        DIV.U W2,W4
        MOV W0,SPEED
        REPEAT #9
        LSR SPEED
        CLR TMR2
        CLR TMR3
        RETURN

        ;<<insert more user code here>>





done:
        BRA     done              ;Place holder for last line of executed code



;..............................................................................
;Subroutine: Initialization of W registers to 0x0000
;..............................................................................

_wreg_init:
        CLR W0
        MOV W0, W14
        REPEAT #12
        MOV W0, [++W14]
        CLR W14
        RETURN



;..............................................................................
;Timer 1 Interrupt Service Routine
;Example context save/restore in the ISR performed using PUSH.D/POP.D
;instruction. The instruction pushes two words W4 and W5 on to the stack on
;entry into ISR and pops the two words back into W4 and W5 on exit from the ISR
;..............................................................................

__T1Interrupt:
        PUSH.D W4                  ;Save context using double-word PUSH

        ;<<insert more user code here>>

        BCLR IFS0, #T1IF           ;Clear the Timer1 Interrupt flag Status
                                   ;bit.
        POP.D W4                   ;Retrieve context POP-ping from Stack
        RETFIE                     ;Return from Interrupt Service routine

          

;--------End of All Code Sections ---------------------------------------------
        .end
                               ;End of program code in this file