; *************************************************************************
;
;    Filename: STMD.asm
;
;    Date: 09/18/05
;
;    Author: Aaron Garber 
;
;   Things to do:
;   --  Implement test mode to read pot for speed setting?
;
;   Fuses:  (checkmark only the following boxes)
;   -- Preserve EEPROM memory through Chip Erase Cycle [EESAVE=0]  ; fault codes are logged to EEPROM
;   -- Brown-out detection level at VCC=4.3 V; [BODLEVEL=100]
;   -- Int. RC Osc. 8 MHz; Start-up time PWRDWN/RESET:6CK/14CK + 65 ms; [CKSEL=0010 SUT=10]
;
; *************************************************************************
;
;   License: 
;   
;   This software can be used and modified for personal or non-commercial use only. 
;   
;   Please contact avr.stmd@gmail.com if you wish to use this code
;   for any commercial purpose.
;
;   Please contact avr.stmd@gmail.com with any bug fixes or functional improvements.
;                                                                       
;   Use this code at your own risk as there is no guarantee that it will work for you.
;
; *************************************************************************


; ***** Include Files ****************************************************
.include "m48def.inc"
.include "step_seq.asm"             ; stepping sequences
.include "mode_init.asm"


; ***** Defines **********************************************************
.def    fault_code = r0             ; value of fault code to be stored in EEPROM
.def    isr_SREG = r1               ; temporary storage of SREG in ISR's
;.def    = r2                       ; not used
;.def    = r3                       ; not used
;.def    = r4                       ; not used
;.def    = r5                       ; not used
;.def    = r6                       ; not used
;.def    = r7                       ; not used
;.def    = r8                       ; not used
;.def    = r9                       ; not used
;.def    = r10                      ; not used
;.def    = r11                      ; not used
;.def    = r12                      ; not used
;.def    = r13                      ; not used
;.def    = r14                      ; not used
.def    temp3 = r15                 ; temporary variable for general use
.def    temp1 = r16                 ; temporary variable for general use
.def    temp2 = r17                 ; temporary variable for general use
.def    max_entries = r18           ; number of byte entries in currently selected stepping sequence, zero based
.def    current_position = r19      ; current position in step sequence
.def    tim0_LEDrolls = r20         ; actual number of roll-overs for timer0, used to toggle LED on 500 ms tick
.def    tim2_idlerolls = r21        ; actual number of roll-overs for timer2, used to enable idle current reduction after 3 seconds of no step activity
.def    isr_temp1 = r22             ; temporary variable for ISR's
.def    isr_temp2 = r23             ; temporary variable for ISR's
.def    seq_start_L = r24           ; pointer to start of stepping sequence, low byte
.def    seq_start_H = r25           ; pointer to start of stepping sequence, high byte
;.def   XL = r26
;.def   XH = r27
;.def   YL = r28
;.def   YH = r29
;.def   ZL = r30
;.def   ZH = r31


; ***** Equates **********************************************************
.equ    brake_in = 3                ; PC3
.equ    test_in = 4                 ; PC4

.equ    step_in = 2                 ; PD2
.equ    dir_in = 3                  ; PD3
.equ    enable_in = 4               ; PD4
.equ    alive_LED = 5               ; PD5
.equ    brake_out = 6               ; PD6
.equ    idle_red_out = 7                ; PD7
;.equ    tim0_LEDrolls_target = ?   ; see timer0 setup
;.equ    tim2_idlerolls_target = ?  ; see timer2 setup


; ***** Variables in SRAM ************************************************
.dseg
;    var1:   .byte   1
;    var2:   .byte   2

; ***** Data in EEPROM ***************************************************
.eseg
    ee_const:   .db     0   ; normally point here to minimize chance of corruption
    ee_ptr:     .db     2   ; pointer to next empty location in EEPROM  

; ***** The Program ******************************************************
.cseg
.org 0x0000
    rjmp    rst         ; reset handler
    rjmp    ext_int0    ; IRQ0 handler
    rjmp    ext_int1    ; IRQ1 handler
    rjmp    pincint0    ; PCINT0 handler
    rjmp    pincint1    ; PCINT1 handler
    rjmp    pincint2    ; PCINT2 handler
    rjmp    WDT         ; Watchdog timer handler
    rjmp    tim2_compa  ; timer2 compare A handler
    rjmp    tim2_compb  ; timer2 compare B handler
    rjmp    tim2_ovf    ; timer2 overflow handler
    rjmp    tim1_capt   ; timer1 capture handler
    rjmp    tim1_compa  ; timer1 compare A handler
    rjmp    tim1_compb  ; timer1 compare B handler
    rjmp    tim1_ovf    ; timer1 overflow handler
    rjmp    tim0_compa  ; timer0 compare A handler
    rjmp    tim0_compb  ; timer0 compare B handler
    rjmp    tim0_ovf    ; timer0 overflow handler
    rjmp    spi_stc     ; SPI transfer complete handler
    rjmp    usart_rxc   ; USART, RX complete handler
    rjmp    usart_udre  ; USART, UDR empty handler
    rjmp    usart_txc   ; USART, TX complete handler
    rjmp    ADC_com     ; ADC conversion complete handler
    rjmp    ee_rdy      ; EEPROM ready handler
    rjmp    ana_comp    ; analog comparator handler
    rjmp    twi         ; 2-wire serial interface handler
    rjmp    spm_rdy     ; store program memory ready handler

; ************************************************************************
ext_int0:    ; IRQ0 handler
    in  isr_SREG, SREG          ; save status register

    sbi portd, idle_red_out     ; disable idle current reduction
    clr tim2_idlerolls          ; reset idle current reduction rollover counter
    ldi isr_temp1,  0b00000001  ; re-enable idle current reduction timer
                    ; xxxxx---  ; reserved
                    ; -----0--  ; 0 = timer/counter2 Output Compare B Match interrupt disabled
                    ; ------0-  ; 0 = timer/counter2 Output Compare A Match interrupt disabled
                    ; -------0  ; 0 = timer/counter2 Overflow interrupt disabled
    sts TIMSK2, isr_temp1
    
    mov ZL, seq_start_L         ; initialize ZL and ZH
    mov ZH, seq_start_H

    sbic pind, dir_in           ; check if direction pin is high...
    rjmp forward                ; ...yes, forward direction

reverse:
    clr isr_temp1
    dec current_position                ; decrement to next position...
    dec current_position                ; ...requires two decrements to make the word entries into byte equivalent address
    cp  max_entries, current_position   ; check if we are past the beginning of the sequence...
        brsh calc_Z                     ; ...no, then read data
    mov current_position, max_entries   ; ...yes, then point to last set of data entries
    rjmp calc_Z

forward:
    inc current_position                ; increment to next position...
    inc current_position                ; ...requires two increments to make the word entries into byte equivalent address
    cp  max_entries, current_position   ; check if we are we past the end of the sequence...
        brsh calc_Z                     ; ...no, then read data
    clr current_position                ; ...yes, then point back to beginning

calc_Z:                                 ; use the current_position to calculate the Z pointer to new sequence
    clr isr_temp1
    add zl, current_position            ; 16 bit addition to point to the new sequence
    adc zh, isr_temp1

read:                                   ; read the step sequence data 
    lpm isr_temp1, Z+                   ; read new data for dir pins, will be 0b000000xx where xx is the new data
    in  isr_temp2, portd                ; get a copy of portd to work with
    andi isr_temp2, 0b11111100          ; clear direction pins
    or  isr_temp1, isr_temp2            ; OR in the new data
    out portd, isr_temp1                ; update portb
        
    lpm isr_temp1, Z                    ; read new data for M pins
    out portb, isr_temp1                ; update portb
    
    out SREG, isr_SREG                  ; restore status register
    reti
; ************************************************************************
ext_int1:    ; IRQ1 handler
    ldi     temp1, 3   ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
pincint0:    ; PCINT0 handler
    ldi     temp1, 4   ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
pincint1:    ; PCINT1 handler
    ldi     temp1, 5   ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
pincint2:    ; PCINT2 handler
    ldi     temp1, 6        ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
WDT:         ; Watchdog timer handler
    ldi     temp1, 7   ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
tim2_compa:  ; timer2 compare A handler
    ldi     temp1, 8   ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
tim2_compb:  ; timer2 compare B handler
    ldi     temp1, 9   ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
tim2_ovf:    ; timer2 overflow handler
    in  isr_SREG, SREG      ; save status register
    inc tim2_idlerolls
    out SREG, isr_SREG      ; restore status register
    reti

; ************************************************************************
tim1_capt:   ; timer1 capture handler
    ldi     temp1, 11  ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
tim1_compa:  ; timer1 compare A handler
    rcall   ext_int0  ; simulate a step in test mode
    reti

; ************************************************************************
tim1_compb:  ; timer1 compare B handler
    ldi     temp1, 13  ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti
 
; ************************************************************************
tim1_ovf:    ; timer1 overflow handler
    ldi     temp1, 14  ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
tim0_compa:  ; timer0 compare A handler
    ldi     temp1, 15  ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
tim0_compb:  ; timer0 compare B handler
    ldi     temp1, 16  ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
tim0_ovf:    ; timer0 overflow handler
    in  isr_SREG, SREG      ; save status register
    inc tim0_LEDrolls
    out SREG, isr_SREG      ; restore status register
    reti

; ************************************************************************
spi_stc:     ; SPI transfer complete handler
    ldi     temp1, 18  ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
usart_rxc:   ; USART, RX complete handler
    ldi     temp1, 19  ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
usart_udre:  ; USART, UDR empty handler
    ldi     temp1, 20  ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
usart_txc:   ; USART, TX complete handler
    ldi     temp1, 21  ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; *************************************************************************
ADC_com:     ; ADC conversion complete handler
    ldi     temp1, 22  ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; *************************************************************************
ee_rdy:      ; EEPROM ready handler
    ldi     temp1, 23  ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
ana_comp:    ; analog comparator handler
    ldi     temp1, 24  ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
twi:         ; 2-wire serial interface handler
    ldi     temp1, 25  ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti


; ************************************************************************
spm_rdy:     ; store program memory ready handler
    ldi     temp1, 26  ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap
    reti

; ************************************************************************
rst:
    ldi r16, high(RAMEND)   
    out SPH,r16             ; set stack pointer to top of RAM
    ldi r16, low(RAMEND)
    out SPL,r16

; ************************************************************************
; configure Port B          ; this port drives the Mx pins of the two LMD18245T driver chips
    ldi temp1,  0b11111111
                ; 1-------  ; 1 = PB7 is ouput, B Driver - M4
                ; -1------  ; 1 = PB6 is ouput, B Driver - M3
                ; --1-----  ; 1 = PB5 is ouput, B Driver - M2
                ; ---1----  ; 1 = PB4 is ouput, B Driver - M1
                ; ----1---  ; 1 = PB3 is ouput, A Driver - M4
                ; -----1--  ; 1 = PB2 is ouput, A Driver - M3
                ; ------1-  ; 1 = PB1 is ouput, A Driver - M2
                ; -------1  ; 1 = PB0 is ouput, A Driver - M1
    out DDRB,   temp1

    ldi temp1,  0b00000000
                ; 0-------  ; 1 = PB7 initially low, B Driver - M4 
                ; -0------  ; 1 = PB6 initially low, B Driver - M3
                ; --0-----  ; 1 = PB5 initially low, B Driver - M2
                ; ---0----  ; 1 = PB4 initially low, B Driver - M1
                ; ----0---  ; 1 = PB3 initially low, A Driver - M4
                ; -----0--  ; 1 = PB2 initially low, A Driver - M3
                ; ------0-  ; 1 = PB1 initially low, A Driver - M2
                ; -------0  ; 1 = PB0 initially low, A Driver - M1
    out PORTB,  temp1

; ************************************************************************
; configure Port C
    ldi temp1,  0b00000000
                ; x-------  ; reserved
                ; -x------  ; PC6 is /Reset, pulled high via 10k
                ; --0-----  ; 0 = PC5 is input, spare input
                ; ---0----  ; 0 = PC4 is input, DIP switch to gnd, test mode select
                ; ----0---  ; 0 = PC3 is input, DIP switch to gnd, disable mode select: Do we let the motors freewheel, or turn braking on?
                ; -----0--  ; 0 = PC2 is input, DIP switch to gnd, step mode select2
                ; ------0-  ; 0 = PC1 is input, DIP switch to gnd, step mode select1
                ; -------0  ; 0 = PC0 is input, DIP switch to gnd, step mode select0
    out DDRC,   temp1

    ldi temp1,  0b01111111
                ; x-------  ; reserved
                ; -1------  ; 1 = PC6 internal pullup enabled
                ; --1-----  ; 1 = PC5 internal pullup enabled
                ; ---1----  ; 1 = PC4 internal pullup enabled
                ; ----1---  ; 1 = PC3 internal pullup enabled
                ; -----1--  ; 1 = PC2 internal pullup enabled
                ; ------1-  ; 1 = PC1 internal pullup enabled
                ; -------1  ; 1 = PC0 internal pullup enabled
    out PORTC,  temp1

; ************************************************************************
; configure Port D
    ldi temp1,  0b11100011  
                ; 1-------  ; 1 = PD7 is output, idle current reduction enable, 1k0 resistor to DAC_REF pins on drivers.
                            ;                    DAC_REF pins on drivers are pulled high thru 1k0, so bringing this pin low
                            ;                    sets up a voltage divider, thereby reducing the DAC_REF voltage
                ; -1------  ; 1 = PD6 is output, motor brake control, connects to BRAKE pins on drives thru a removeable jumper, goes high to turn braking on
                ; --1-----  ; 1 = PD5 is output, aliveLED to gnd thru 470R, toggles on a 500mS basis
                ; ---0----  ; 0 = PD4 is input, enable input from opto-isolator, pulled high through 270R, goes low when 5v is applied to input of opto-isolator
                ; ----0---  ; 0 = PD3 is input, direction input from opto-isolator, pulled high through 270R, goes low when 5v is applied to input of opto-isolator
                ; -----0--  ; 0 = PD2 is input, step input from opto-isolator, pulled high through 270R, goes low when 5v is applied to input of opto-isolator
                ; ------1-  ; 1 = PD1 is output, goes to B driver - Dir pin
                ; -------1  ; 1 = PD0 is output, goes to A Driver - Dir pin
    out DDRD,  temp1

    ldi temp1,  0b10011100
                ; 1-------  ; 1 = idle current reduction initially off
                ; -0------  ; 0 = motor braking initially off
                ; --0-----  ; 0 = aliveLED initially off
                ; ---111--  ; 111 = enable internal pull-ups for optically isolated inputs
                ; ------00  ; 0 = DIR pins on A & B drivers is initially low
    out PORTD,  temp1

; ************************************************************************
; check MCUSR for source of reset
; only power-on reset flag is valid
; all others are trapped

    in      temp1, MCUSR
    sbrc    temp1, PORF     ; power-on reset?
    rjmp    continue

wd_reset:
    sbrs    temp1, WDRF     ; watchdog reset?
    rjmp    brout_reset
    ldi     temp1, 27       ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap

brout_reset:    
    sbrs    temp1, BORF     ; brownout reset?
    rjmp    ext_reset
    ldi     temp1, 28       ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap

ext_reset: 
    sbrs    temp1, EXTRF    ; external reset?
    rjmp    continue
    ldi     temp1, 29       ; fault code
    mov     fault_code, temp1
    rjmp    fault_trap

continue:

; ************************************************************************
; configure timer/counter0  ; we will use timer0 to toggle aliveLED on a 500 ms basis
    ldi temp1,  0b00000000
                ; 00------  ; 00 = normal port operation, OC0A disconnected
                ; --00----  ; 00 = normal port operation, OC0B disconnected
                ; ----xx--  ; reserved
                ; ------00  ; 00 = normal waveform generation mode
    out TCCR0A, temp1

    ldi temp1,  0b00000100
                ; 0-------  ; 1 = force output compare for channel A
                ; -0------  ; 1 = force output compare for channel B
                ; --xx----  ; reserved
                ; ----0---  ; 0 = normal waveform generation mode
                ; -----000  ; 000 = disabled
                ; -----001  ; 001 = no prescaler, internal clock
                ; -----011  ; 011 = 1:64 prescaler, internal clock
                ; -----100  ; 100 = 1:256 prescaler, internal clock
                ; -----101  ; 101 = 1:1024 prescaler, internal clock
    out TCCR0B, temp1

    clr temp1               ; clear some registers
    out TCNT0, temp1
    out OCR0A, temp1
    out OCR0B, temp1

    ; 8Mhz clock -> 125 ns instruction cycle
    ; 8 bit counter, 2^8 x 125 ns = 32 us
    ; with 1:256 prescaler: 256 x 32 us = 8.192 ms per rollover
    ; for 500 ms delay: 500 ms / 8.192 ms = 61.03 rollovers per 500 ms
    ; -- calculate error for 61 rollovers: 61 * 8.192 ms = 499.712 ms 
    ; --- close enough for toggling a LED!!!!
    .equ tim0_LEDrolls_target = 61
    ldi tim0_LEDrolls, 0

; enable/disable timer/counter0 interrupts
    ldi temp1,  0b00000001
                ; xxxxx---  ; reserved
                ; -----0--  ; 0 = timer/counter0 Output Compare B Match interrupt disabled
                ; ------0-  ; 0 = timer/counter0 Output Compare A Match interrupt disabled
                ; -------0  ; 0 = timer/counter0 Overflow interrupt disabled
    sts TIMSK0, temp1

; ************************************************************************
; configure timer/counter1  ; use OC1A to generate timed steps for test mode
    ldi temp1,  0b00000000
                ; 00------  ; normal port operation, OC1A disconnected
                ; --00----  ; normal port operation, OC1B disconnected
                ; ----xx--  ; reserved
                ; ------00  ; 00 = clear timer on compare match with OCR1A
                ; ------00  ; 00 = clear timer on compare match with ICR1
                ; ------00  ; 00 = normal mode
    sts TCCR1A, temp1

    ldi temp1,  0b00001001
                ; 0-------  ; 0 = input capture noise canceler disabled
                ; -0------  ; 0 = trigger capture on falling edge of IC1
                ; --x-----  ; reserved
                ; ---00---  ; 00 = normal mode
                ; ---01---  ; 01 = clear timer on compare match with OCR1A
                ; ---11---  ; 11 = clear timer on compare match with ICR1
                ; -----001  ; 001 = no prescaler, internal clock
                ; -----011  ; 011 = 1:64 prescaler, internal clock
    sts TCCR1B, temp1

    ldi temp1,  0b00000000
                ; 0-------  ; 1 = force output compare for channel A
                ; -0------  ; 1 = force output compare for channel B
                ; --xxxxxx  ; reserved
    sts TCCR1C, temp1
    
    clr temp1               ; clear some registers
    clr temp2         
    sts TCNT1H, temp1       
    sts TCNT1L, temp2
    
    clr temp1               ; clear some registers
    clr temp2         
    sts OCR1AH, temp1       
    sts OCR1AL, temp2
    
    clr temp1               ; clear some registers
    clr temp2         
    sts OCR1BH, temp1       
    sts OCR1BL, temp2
    
; enable/disable timer/counter1 interrupts
    ldi temp1,  0b00000000
                ; --0-----  ; 0 = input capture interrupt disabled
                ; xx-xx---  ; reserved
                ; -----0--  ; 0 = timer/counter2 Output Compare B Match interrupt disabled
                ; ------0-  ; 0 = timer/counter2 Output Compare A Match interrupt disabled
                ; -------0  ; 0 = timer/counter2 Overflow interrupt disabled
    sts TIMSK1, temp1

; ************************************************************************
; configure timer/counter2  ; we will use timer2 enable idle current reduction after 3 seconds of no step activity
    ldi temp1,  0b00000000
                ; 00------  ; 00 = normal port operation, OC0A disconnected
                ; --00----  ; 00 = normal port operation, OC0B disconnected
                ; ----xx--  ; reserved
                ; ------00  ; 00 = normal waveform generation mode
    sts TCCR2A, temp1

    ldi temp1,  0b00000111
                ; 0-------  ; 1 = force output compare for channel A
                ; -0------  ; 1 = force output compare for channel B
                ; --xx----  ; reserved
                ; ----0---  ; 0 = normal waveform generation mode
                ; -----000  ; 000 = disabled
                ; -----001  ; 001 = no prescaler, internal clock
                ; -----100  ; 011 = 1:64 prescaler, internal clock
                ; -----100  ; 110 = 1:256 prescaler, internal clock
                ; -----101  ; 111 = 1:1024 prescaler, internal clock
    sts TCCR2B, temp1
    
    clr temp1               ; clear some registers
    sts TCNT2, temp1
    sts OCR2A, temp1
    sts OCR2B, temp1

    ; 8Mhz clock -> 125 ns instruction cycle
    ; 8 bit counter, 2^8 x 125 ns = 32 us
    ; with 1:1014 prescaler: 1024 x 32 us = 32.768 ms per rollover
    ; for 3 s delay: 3 s / 32.768 ms = 91.55 rollovers per 3 s
    ; -- calculate error for 92 rollovers: 92 * 32.768 ms = 3.015 s
    .equ tim2_idlerolls_target = 92
    ldi tim2_idlerolls, 0

; enable/disable timer/counter2 interrupts
    ldi temp1,  0b00000001
                ; xxxxx---  ; reserved
                ; -----0--  ; 0 = timer/counter2 Output Compare B Match interrupt disabled
                ; ------0-  ; 0 = timer/counter2 Output Compare A Match interrupt disabled
                ; -------0  ; 0 = timer/counter2 Overflow interrupt disabled
    sts TIMSK2, temp1

; ************************************************************************
; configure external interrupts
    ldi temp1, 0b00000010
               ; xxxx----   ; reserved
               ; ----00--   ; 00 = low level on INT1 generates interrupt request
               ; ------10   ; 10 = falling edge on INT0 generates interrupt request
    sts EICRA, temp1

    ldi temp1, 0b00000000
               ; xxxxxx--   ; reserved
               ; ------0-   ; 0 = INT1 disabled
               ; -------0   ; 0 = INT0 disabled
    out EIMSK, temp1


; ************************************************************************
; configure watchdog timer
    
    wdr                     ; hit the dog

    lds temp1, WDTCSR       ; start timed sequence to set up WDTCSR
    ori temp1, (1<<WDCE) | (1<<WDE)
    sts WDTCSR, temp1


    ldi temp1, 0b01000000
               ; 0-------   ; 0 = watchdog interrupt flag clear (WDIF)
               ; -1------   ; 1 = watchdog interrupt enabled (WDIE)
               ; --0--000   ; 0--000 = 16 ms timeout (watchdog prescaler bits)
               ; --1--001   ; 1--001 = 8 sec timeout (watchdog prescaler bits)
               ; ---0----   ; watchdog change enable (WDCE)
               ; ----0---   ; 0 = Watchdog system reset disabled (WDE)
    sts WDTCSR, temp1

; ************************************************************************
; initialize mode of stepping based on DIP switches
    in  temp1,  PINC
    andi temp1, 0b00000111  ; mask off the bits that don't pertain to stepping mode
    
    cpi temp1,  0b111  		; full-step mode with both phases on?  
    brne next1 
    rcall init_full_both
    
next1:
    cpi temp1,  0b110       ; full-step mode with one phase on?
    brne next2
    rcall init_full_wave

next2:    
    cpi temp1,  0b101       ; half-step mode?
    brne next3
    rcall init_half

next3:    
    cpi temp1,  0b100       ; quarter step mode?
    brne next4
    rcall init_quarter

next4:    
    cpi temp1,  0b011       ; sixth step mode?
    brne next5
    rcall init_sixth

next5:    
    cpi temp1,  0b010       ; eighth step mode?
    brne next6
    rcall init_eighth

next6:    
    cpi temp1,  0b001       ; tenth step mode?
    brne next7
    rcall init_tenth

next7:    
    cpi temp1,  0b000       ; twelfth or sixteenth step mode?
    brne next8
    rcall init_twelfth      
;    rcall init_sixteenth

next8:    
    sei         ; enable interrupts after everything is configured


; ************************************************************************      
Loop:   ; loop here forever waiting for step interruputs
    wdr
    rcall    enable_check       ; drive enabled?
    wdr                         ; hit the dog 
    rcall    LED_check          ; time to toggle LED?
    wdr                         ; hit the dog
    rcall    test_mode_check    ; test mode?
    wdr                         ; hit the dog
    rcall    idle_cur_red_check ; check to see if idle current reduction should be turned on
    rjmp     Loop

; ************************************************************************
; ***** Subroutines ******************************************************
; ************************************************************************

; ************************************************************************
LED_check:      ; check timer/counter0 rollovers to see if we should toggle the LED
    cpi  tim0_LEDrolls,  tim0_LEDrolls_target
    brsh toggle_led
    ret    
toggle_led:
    sbi pind, alive_LED ; toggle the pin
    clr tim0_LEDrolls   ; reset rollover counter
    ret

; ************************************************************************    
enable_check:   ; check to see if the drive is enabled *******************
                
; PD4 is the enable input from opto-isolator.  It is pulled high through 270R when disabled
; and goes low when 5v is applied to input of opto-isolator to enable the drive

; If drive is disabled, we want to disable external INT0.
                
; PC3 is an input with internal pullup enabled, DIP switch to gnd.
; When drive is disabled (PD4 high) and PC3 is low, we will let the motors freewheel
; by setting M pins low (PortB) and brake pin(s) (PD6) low.
                
; When drive is disabled (PD4 high) and PC3 is high, we will enable motor braking
; by taking brake pin(s) PD6 high.

    sbic PIND, enable_in    ; drive enabled?
    rjmp drive_disabled

    
drive_enabled:    
    cbi PORTD, brake_out    ; braking off
    sbi EIMSK, INT0         ; enable step interrupts
    ret

drive_disabled:
    cbi EIMSK, INT0         ; disable step interrupts
    sbis PINC, brake_in     ; 
    rjmp brake_on

freewheel:    
    clr temp1
    out PORTB, temp1        ; M pins all low so motor can freewheel
    cbi PORTD, brake_out    ; braking off
    ret

brake_on:    
    sbi PORTD, brake_out    ; braking on    
    ret

; ************************************************************************
test_mode_check: ; is test mode active?  Rotate motor, if yes
    sbic PINC, test_in      ; if it's clear, DIP switch is on so test mode is active
    rjmp test_disabled

    ldi temp1,  0b00000010  ; enable interrupt to periodically fire step signal
                ; --0-----  ; 0 = input capture interrupt disabled
                ; xx-xx---  ; reserved
                ; -----0--  ; 0 = timer/counter2 Output Compare B Match interrupt disabled
                ; ------0-  ; 0 = timer/counter2 Output Compare A Match interrupt disabled
                ; -------0  ; 0 = timer/counter2 Overflow interrupt disabled
    sts TIMSK1, temp1
    ret

test_disabled:    
    ldi temp1,  0b00000000  ; disable interrupt
                ; --0-----  ; 0 = input capture interrupt disabled
                ; xx-xx---  ; reserved
                ; -----0--  ; 0 = timer/counter2 Output Compare B Match interrupt disabled
                ; ------0-  ; 0 = timer/counter2 Output Compare A Match interrupt disabled
                ; -------0  ; 0 = timer/counter2 Overflow interrupt disabled
    sts TIMSK1, temp1
    
    ret

; ************************************************************************
idle_cur_red_check: ; is it time to reduce idle current?
    cpi  tim2_idlerolls, tim2_idlerolls_target
    brsh reduce_current
    ret
reduce_current:
    cli                 ; disable interrupts while we reduce current and stop timer2
    cbi portd, idle_red_out
    clr temp1           ; disable timer2 interrupts
    sts TIMSK2, temp1
    sei                 ; interrupts back on
    ret 

; ************************************************************************
fault_trap:  ; trap and log faults to EEPROM
    
    cli                 ; prevent new interrupts from getting us out of the trap
    
    ldi temp1, EE_PTR   ; EE_PTR contains the address of the next empty space in EEPROM
    rcall EEPROM_read
                            
    in  temp3, EEDR     ; store a copy of the address of the next empty space in EEPROM

    in  temp1, EEDR         ; address to write to
    mov temp2, fault_code   ; data to write
    rcall EEPROM_write
    
    inc temp3           ; calculate the new address of the next empty space in EEPROM
    breq infinite       ; test zero flag for rollover, meaning EEPROM is full

    ldi temp1, EE_PTR   ; address to write to
    mov temp2, temp3    ; data to write
    rcall EEPROM_write

infinite:
    cbi  portd, alive_LED   ; on to indicate fault
    rjmp infinite           ; spin forever...

; ************************************************************************
EEPROM_read:  ; read EEPROM
    
; Call: Address to read contained in temp1
    
    sbic    EECR, EEPE      ; wait for completion of previous write
    rjmp    EEPROM_read
    
    out     EEARL, temp1    ; setup low address byte
    ldi     temp1, 0        ; setup high address byte...
    out     EEARH, temp1    ; ...always zero as there are only 255 bytes on M48

    sbi     EECR, EERE      ; start the read by writing EERE

    ldi     temp1, 0
    out     EEARH, temp1    ; point back to location 0...
    out     EEARL, temp1    ; ...to minimize chance of corruption

    ret

; ************************************************************************
EEPROM_write:   ; write EEPROM
    
; Call: Address to write contained in temp1
;       Data to write contained in temp2    
    
    sbic    EECR, EEPE      ; wait for completion of previous write
    rjmp    EEPROM_write
    
    out     EEARL, temp1    ; setup low address byte
    ldi     temp1, 0        ; setup high address byte...
    out     EEARH, temp1    ; ...always zero as there are only 255 bytes on M48

    out     EEDR, temp2     ; setup data register
    
    sbi     EECR, EEMPE     ; set master write enable
    sbi     EECR, EEPE      ; set write enable
    
    ldi     temp1, 0
    out     EEARH, temp1    ; point back to location 0...
    out     EEARL, temp1    ; ...to minimize chance of corruption

    ret