;***********************************************************************
;*              Stepper motor controller
;*              Version for ESO MASS centring device
;*              Circuit diagrams:       pbl-motor.sch
;*              Circuit boards:         pbl-motor.pcb
;*              Last modification:      31.10.03
;***********************************************************
; ATTENTION !!!! Commands were changed with respect versions: motor_t and motor_tn
;Число микрошагов на шаг - 4, 16 микрошага на цикл
;       Минимальная          - 100пш/с = 400 мш/с (2500мкс)
;       Номинальная скорость - 200пш/с = 800 мш/с (1250мкс)
;       Максимальная         - 1000пш/с = 4000 мш/с (250мкс)
;       Тактовая частота     -  14.7456 Мгц = 0.0678 мкс
;       Значение пределителя -   8   =  0.5425 мкс
;
;       Таблица  sin    cos
;       POINTER = 0 - 15.= ABS_POS&(4*MICRO-1)
;
.include "M8DEF.INC"
.include "motor_eso.inc"
.listmac
.equ    BLS=    0x0e00
.macro  skip
        rjmp    PC+2
.endmacro

;***********************************************************************
;*                  Interrupt table
;***********************************************************************
.cseg
.org $0000
        rjmp    Start                   ; Programm start, Reset handler
.org INT0addr
        rjmp    line_monitor            ; RS485 line monitor handler
.org INT1addr
        reti
.org OC2addr
        rjmp    switch_timeout          ; Switch timeout handler
.org OVF2addr
        rjmp    window_timeout          ; Window timeout handler
.org ICP1addr
	reti
.org OC1Aaddr
        rjmp    free_motion             ; Step timing handler
.org OC1Baddr
	reti
.org OVF1addr
	reti
.org OVF0addr
        rjmp    long_cycle              ; Cycle for temperature and voltage control
.org SPIaddr
	reti
.org URXCaddr
        rjmp    receiver                ; UART receiver handler
.org UDREaddr
	reti
.org UTXCaddr
        rjmp    transmitt_complete      ; UART transmitter handler
.org ADCCaddr
        reti
.org ERDYaddr
	reti
.org ACIaddr
        reti
.org TWIaddr
	reti
.org SPMaddr
        reti
;***********************************************************************
;*                  Constant tables
;***********************************************************************
identno:                                ; Device Identification
        .dw     iddev
        .dw     pdate

line:
        .db     MOTOR, fbaudrate         ; Line constants
        .db     fbottom, fswitch

apparatus:                              ; Constants
        .db     scale1, scale2
        .dw     freq

defaults:
        .dw     dspeed
;        .dw     dacc_path
;        .dw     dacceleration
enddefaults:

sinus:
        .db    180,  236,  255, 236
        .db    180,   98,    0,  98
        .db    180,  236,  255, 236
        .db    180,   98,    0,  98
phases:
        .db      0,    0,    0,   0
        .db      0,    0,    5,   1
        .db      1,    1,    1,   1
        .db      1,    1,    4,   0

;***********************************************************************
;*                  Command decoding
;***********************************************************************
;*      000 - (0x00) - Data  block
;*            (0x10) - Data block with marker
;*   +  001 - (0x20) - Argument write into SRAM (Byte)
;*            (0x30) - Arguments write into SRAM (2 Byte)
;*      010 - (0x40) - Command with 1-byte argument
;*            (0x50) - Command with 2-bytes argument
;*      011 - (0x60) - Argument write into EEPROM {not implemented)
;*            (0x70)
;*   +  100 - (0x80) - Direct Functions
;*            (0x90) - Direct Functions
;*   +  101 - (0xA0) - Request functions
;*            (0xB0) - Reserve
;*      110 - (0xC0) - Special functions
;*            (0xD0) - Reserve
;*   +  111 - (0xE0) - Parameter read from SRAM  (Byte)
;*            (0xF0) - Parameters read from SRAM  (2 Byte)
;***********************************************************************
command:
        ldi     ZL,low(rbuffer+1)
        ld      COMMAND,Z+
        cpi     COMMAND,0x20
        brcs    commandgrup
        ld      AL,Z+
        ld      AH,Z+
        mov     ARGUMENT,COMMAND
        ldi     TEMP,0x0F               ; Argument address extraction
        and     ARGUMENT,TEMP
        swap    COMMAND
        and     COMMAND,TEMP
        mov     ZL,COMMAND
        lsr     ZL
        subi    ZL,low(-commandgrup)
        ijmp
commandgrup:                            ; Command groups separation
        rjmp    none                    ; Data blocks loading
        rjmp    parameterset
        rjmp    parameterfunction
        rjmp    none

        rjmp    simplefunction
        rjmp    datafunction
        rjmp    special                 ; C0-D0
        rjmp    parameterget

simplefunction:                         ; Direct functions decoding
        ldi     ZL,low(sfunction)
        add     ZL,ARGUMENT
        ijmp
sfunction:
        rjmp    turn_on                 ; Motor on 			(0x80)
	rjmp    turn_off                ; Motor off                     (0x81)
	rjmp    test_motion             ; Motor status check            (0x82)
        rjmp    comm_left               ; Motion to left stop           (0x83)
        rjmp    comm_right              ; Motion to right stop          (0x84)
        rjmp    stop_motion             ; Emergency stop                (0x85)
        rjmp    none
        rjmp    soft_reset              ; Software reset                (0x87)

        rjmp    comm_sens_on            ; Sensor LED turn on            (0x88)
        rjmp    comm_sens_off           ; Sensor LED turn off           (0x89)
        rjmp    comm_null               ; reset of abs position         (0x8A)
        rjmp    none                    ; ACN Signal
        rjmp    none                    ; ACN Signal
        rjmp    none                    ; ACN Signal
        rjmp    none                    ; ACN Signal
        rjmp    none                    ; ACN Signal

datafunction:                           ; Request functions decoding
        ldi     ZL,low(dfunction)
        add     ZL,ARGUMENT
        ijmp
dfunction:
        rjmp    none
        rjmp    none
        rjmp    identification          ; Module identification request
        rjmp    constants               ; Module constants request
        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    none

special:
        tst     ARGUMENT
        brne    PC+2
        rjmp    flash_update            ; Switch to FlashWriter (0xC0)
        rjmp    none

parameterset:                           ; Argument(s) write into SRAM
parameterfunction:                      ; Command with argument(s)
        ldi     ZL,low(parameters)
        add     ZL,ARGUMENT
        st      Z+,AL
        sbrc    COMMAND,0               ; 1-byte or 2-bytes?
        st      Z+,AH
        sbrs    COMMAND,2               ; Write or write and execute
        rjmp    yes
        sbrc    STATUS,MOTION
        rjmp    busy
        ldi     ZL,low(pfunction)
        add     ZL,ARGUMENT
        ijmp
pfunction:                              ; Associated command decoding
        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    comm_shift_at
        rjmp    none
        rjmp    load_speed
        rjmp    none

        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    none
        rjmp    none

parameterget:                           ; Parameter(s) read from SRAM
        ldi     ZL,low(parameters)
        st      Z,STATUS
        add     ZL,ARGUMENT
        ldi     TEMP_CNT,2
        sbrc    COMMAND,0
        inc     TEMP_CNT
        ldi     YL,low(tbuffer-1)
        st      Y+,TEMP_CNT
        dec     TEMP_CNT
        st      Y+,TEMP_CNT
        rcall   rr_copy
        st      Y+,YH
        rjmp    crc_gen

none:   ldi     TRANSMIT,ACN            ; ACN Signal
        rjmp    signal

yes:    ldi     TRANSMIT,ACY            ; ACY Signal
        rjmp    signal

busy:   ldi     TRANSMIT,ACW            ; ACW Signal
        rjmp    signal

step_check:
        sbic    PORTD,LED_ON            ; Check sensor LED activity
        rjmp    check_fin
        ldi     YL,left_chatter
        ld      ACCUM,Y
        sbic    PIND,SENS_L             ; First zero at pin -  set LEFT in sensor
        rjmp    clef1                   ; and stop and lock left motion
	sbr	SENSOR,(1<<LEFT)
        clr     ACCUM
        sbrc    STATUS,LEFT
        rjmp    right_sensor            ; If LEFT is set already - do nothing
        sbr     STATUS,(1<<LEFT)
        cbr     STATUS,(1<<RIGHT)
        sbrc    STATUS,FORWARD
        rjmp    right_sensor
        sbrc    STATUS,MOTION           ; Left motion off if MOTION=1 and FORWARD=0
        rcall   do_stop
        rjmp    right_sensor
clef1:  cbr	SENSOR,(1<<LEFT)
	sec                             ; Eighth one at pin -  clear LEFT in sensor
        rol     ACCUM
        brcc    right_sensor
        cbr     STATUS,(1<<LEFT)
right_sensor:
        st      Y+,ACCUM
        ld      ACCUM,Y
        sbic    PIND,SENS_R             ; First zero at pin -  set RIGHT in sensor
        rjmp    crig1                   ; and stop and lock right motion
	sbr	SENSOR,(1<<RIGHT)
        clr     ACCUM
        sbrc    STATUS,RIGHT
        rjmp    check_end               ; If RIGHT is set already - do nothing
        sbr     STATUS,(1<<RIGHT)
        cbr     STATUS,(1<<LEFT)
        sbrs    STATUS,FORWARD
        rjmp    check_end
        sbrc    STATUS,MOTION           ; Right motion off if MOTION=1 and FORWARD=1
        rcall   do_stop
        rjmp    check_end
crig1:  cbr	SENSOR,(1<<RIGHT)
	sec                             ; Eighth one(or 128th?) at pin -  clear RIGHT in sensor
        rol     ACCUM
        brcc    check_end
        cbr     STATUS,(1<<RIGHT)
check_end:
        st      Y,ACCUM
        sts     sens_status,SENSOR
check_fin:
        cbr     STATUS,(1<<NEW_STEP)
        rjmp    mainf

;***********************************************************************
;*                  Main
;***********************************************************************
Start:                                  ; Entry
        ldi     TEMP,low(RAMEND)        ; Stack Pointer initialization
        out     SPL,TEMP
        ldi     TEMP,high(RAMEND)
        out     SPH,TEMP

        ldi     TEMP,BDIRECT            ; Port B tuning
        out     DDRB,TEMP
        ldi     TEMP,BMASK
        out     PORTB,TEMP

        ldi     TEMP,CDIRECT            ; Port C setting
        out     DDRC,TEMP
        ldi     TEMP,CMASK
        out     PORTC,TEMP

        ldi     TEMP,DDIRECT            ; Port D setting
        out     DDRD,TEMP
        ldi     TEMP,DMASK
        out     PORTD,TEMP

        rcall   sram_initialization     ; Copy data into SRAM (YH=0, ZH=0)
        rcall   exchange_initialization ; Initialization of the exchange features (XH=0)
        rcall   work_initialization     ; Initialization  of the functional features

        ldi     LINE_CSR,1<<LINE_BUSY
        sei
main:   sbrc    STATUS,NEW_STEP
        rjmp    step_check
mainf:  sbrc    LINE_CSR,FAILURE
        rcall   transmit_repeat
        sbrs    LINE_CSR,RECV_READY     ;Ожидание внешней команды
        rjmp    main
        cbr     LINE_CSR,1<<RECV_READY
        rcall   crc_check
        brne    main
        rcall   command
        rjmp    main

flash_update:                           ; Moving interrupts to boot section
        ldi     TRANSMIT,ACY
        rcall   signal
        sbrc    LINE_CSR,LINE_BUSY
        rjmp    PC-1
        jmp     BLS

soft_reset:
        ldi     TRANSMIT,ACY
        rcall   signal
        sbrc    LINE_CSR,LINE_BUSY
        rjmp    PC-1
        rjmp    Start

;***********************************************************************
;*                      Interrupt handlers
;***********************************************************************
long_cycle:                             ; Cycle for temperature and voltage control
        in      SREG_COPY,SREG
        ldi     TREG,fcycle
        out     TCNT0,TREG
;        sbr     STATUS,(1<<NEW_STEP)
        out     SREG,SREG_COPY
        reti

dofinish:
        cbr     STATUS,(1<<MOTION)
        in      TREG,TIMSK              ; Timer 1 comparator interrupt disable
        cbr     TREG,(1<<OCIE1A)
        out     TIMSK,TREG
        sbi     PORTD,LED_ON            ; Put out sensor LEDs
        rjmp    sy4

free_motion:
        in      SREG_COPY,SREG
        push    SREG_COPY
        push    YL
        push    YH
        push    ZL
        ldi     YL,1
        clr     YH
        sbiw    COUNTL,0                ; Motion finish check COUNT=0?
        breq    dofinish                ; Yes, do finish
        sbiw    COUNTL,1                ; Decrease COUNT
        sbr     STATUS,(1<<NEW_STEP)
        sbrc    STATUS,FORWARD
        rjmp    ma0
        sub     ABS_POSL,YL             ; Absolute position decrease by Step value
        sbc     ABS_POSH,YH
        rjmp    ma1
ma0:    add     ABS_POSL,YL             ; Absolute position increase by Step value
        adc     ABS_POSH,YH
ma1:    ldi     ZL,position
        st      Z+,ABS_POSL
        st      Z+,ABS_POSH
                                         ; New step begin
        mov     ZL,ABS_POSL              ; Выбор и загрузка SIN на 1 обмотку
        andi    ZL,(4*MICRO-1)
        subi    ZL,low(-2*sinus)
        lpm     YL,Z
        subi    ZL,low(-4*MICRO)
        lpm     YH,Z
        in      TREG,PORTC
        cbr     TREG,(1<<PHASE1)|(1<<DIS1)
        or      TREG,YH
        cbi     PORTB,LOAD              ; Select
        sbi     PORTB,SCK
        cbi     PORTB,SCK               ; Adress 0b00
        sbi     PORTB,MOSI
        sbi     PORTB,SCK
        cbi     PORTB,SCK
        sbi     SPCR,SPE
        out     SPDR,YL
        sbis    SPSR,SPIF
        rjmp    PC-1
        cbi     PORTB,MOSI
        sbi     PORTB,LOAD              ; Level is changed here
        cbi     SPCR,SPE
        out     PORTC,TREG
                                         ; Выбор и загрузка COS на 2 обмотку
        mov     ZL,ABS_POSL
        subi    ZL,low(-MICRO)
        andi    ZL,(4*MICRO-1)
        subi    ZL,low(-2*sinus)
        lpm     YL,Z
        subi    ZL,low(-4*MICRO)
        lpm     YH,Z
        lsl     YH
        in      TREG,PORTC
        cbr     TREG,(1<<PHASE2)|(1<<DIS2)
        or      TREG,YH

        cbi     PORTB,LOAD              ; Select
        sbi     PORTB,SCK
        cbi     PORTB,SCK               ; Adress 0b01
        sbi     PORTB,SCK
        cbi     PORTB,SCK
        sbi     SPCR,SPE
        out     SPDR,YL
        sbis    SPSR,SPIF
        rjmp    PC-1
        sbi     PORTB,LOAD              ; Level is changed here
        cbi     SPCR,SPE
        out     PORTC,TREG
                                        ; New step  end
sy4:    pop     ZL
        pop     YH
        pop     YL
        pop     SREG_COPY
        out     SREG,SREG_COPY
        reti

;***********************************************************************
;*                      Interrupt handlers for RS485 exchange
;***********************************************************************
receiver:
        in      SREG_COPY,SREG
        sbis    UCSRB,RXB8              ; Packet header check
        rjmp    receiv_continue         ; It is not header!
        in      RECEIVE,UDR
        tst     RECEIVE
        brpl    receiv_adress           ; It is address byte
        sbrs    LINE_CSR,DATA_SENDING    ; Expected signal?
        rjmp    receiv_return           ; No
        cbr     LINE_CSR,1<<DATA_SENDING
        cpi     RECEIVE,ACK
        breq    receiv_return
        sbr     LINE_CSR,(1<<FAILURE)
        rjmp    receiv_return
receiv_adress:                          ; New transaction
        cbr     LINE_CSR,(1<<DATA_SENDING)|(1<<RECEIVING)
        andi    RECEIVE,MAXADRESS
        lds     BYTE_CNT,adress
        cpse    RECEIVE,BYTE_CNT        ; The module address
        rjmp    receiv_return
        sbr     LINE_CSR,1<<RECEIVING   ; Our packet begin
        ldi     XL,low(rbuffer)
        st      X+,RECEIVE
        clr     BYTE_CNT
        rjmp    receiv_return
receiv_continue:
        in      RECEIVE,UDR
        sbrs    LINE_CSR,RECEIVING
        rjmp    receiv_return
        tst     BYTE_CNT                ; Next bytes from our packet
        brne    rec_end
        cpi     RECEIVE,0x20            ; Packet length computation
        brcs    datum
        brlt    noarg
        inc     BYTE_CNT
        sbrc    RECEIVE,MODIFICATOR
        inc     BYTE_CNT
noarg:  inc     BYTE_CNT
        inc     BYTE_CNT                ; For CRC byte
        sts     rnumber,BYTE_CNT
rec_end:st      X+,RECEIVE
        dec     BYTE_CNT
        brne    receiv_return
rec_zero:
        sbr     LINE_CSR,1<<RECV_READY  ; This packet for module itself
        cbr     LINE_CSR,1<<RECEIVING
receiv_return:
        out     TCNT2,BOTTOM            ; Перезапуск, счетчик уже запущен Проверить!!!!
        ldi     XH,(1<<OCIE2)|(1<<TOIE2)
        out     TIFR,XH
        clr     XH
        out     SREG,SREG_COPY
        reti
datum:  mov     BYTE_CNT,RECEIVE
        tst     BYTE_CNT
        breq    rec_zero
        inc     BYTE_CNT
        inc     BYTE_CNT
        sts     rnumber,BYTE_CNT
        rjmp    rec_end

transmitt_complete:
        in      SREG_COPY,SREG
        cbi     UCSRB,TXB8
        dec     BYTE_CNT
        brpl    transm_continue
        cbi     UCSRB,TXCIE
        cbi     PORTD,DE                ; Disconnect from line
        out     TCNT2,BOTTOM            ; Line timer start
        ldi     XH,(1<<CS22)
        out     TCCR2,XH
        ldi     XH,(1<<OCIE2)|(1<<TOIE2)
        out     TIFR,XH
        clr     XH
        out     SREG,SREG_COPY
        reti
transm_continue:
        ld      TRANSMIT,X+
        out     UDR,TRANSMIT
        out     SREG,SREG_COPY
        reti

line_monitor:
        in      SREG_COPY,SREG
        in      XH,GIMSK                ;запрет прерывания по стартовому биту
        cbr     XH,(1<<INT0)
        out     GIMSK,XH
        sbr     LINE_CSR,(1<<LINE_BUSY) ;установка флага LINE_BUSY
        out     TCNT2,BOTTOM            ; Run Timer2
        ldi     XH,(1<<CS22)
        out     TCCR2,XH
        ldi     XH,(1<<OCIE2)|(1<<TOIE2)
        out     TIFR,XH
        clr     XH
        out     SREG,SREG_COPY
        reti

switch_timeout:                         ; Switch timeout handler
        in      SREG_COPY,SREG
        sbrc    LINE_CSR,DATA_SENDING   ; Signal transmission or
        rjmp    data_trans
        sbi     UCSRB,TXB8              ; data transmission start
        clr     BYTE_CNT
data_trans:
        sbi     PORTD,DE                ; Connect transmitter to line
        out     UDR,TRANSMIT
        sbi     UCSRB,TXCIE
        out     TCCR2,XH                ; Timer 2 stop
        in      XH,TIMSK                ; Switch interrup disable
        cbr     XH,(1<<OCIE2)
        out     TIMSK,XH
        clr     XH
        out     SREG,SREG_COPY
        reti

window_timeout:                         ; Window timeout handler
        in      SREG_COPY,SREG
        sbrc    LINE_CSR,RECEIVING
        rjmp    gap                     ; Receiving packet break
        sbrc    LINE_CSR,DATA_SENDING
        rjmp    sloss                   ; Replied signal loss
line_free:
        cbr     LINE_CSR,1<<LINE_BUSY   ; Line free
        ldi     XH,(1<<INT0)
        out     GIFR,XH
        in      XH,GIMSK                ; Line monitor enabled
        sbr     XH,(1<<INT0)
        out     GIMSK,XH
        clr     XH
        out     TCCR2,XH                ; Timer stop
        out     SREG,SREG_COPY
        reti

sloss:  cbr     LINE_CSR,1<<DATA_SENDING
        sbr     LINE_CSR,1<<FAILURE
        rjmp    line_free

gap:    cbr     LINE_CSR,1<<RECEIVING
;        ldi     TRANSMIT,NAK
;        rcall   signal+1
        rjmp    line_free

;***********************************************************************
;               Exchange subroutines
;***********************************************************************

transmit_repeat:
        cbr     LINE_CSR,(1<<FAILURE)
        ld      TEMP_CNT,X
        dec     TEMP_CNT                ; MAXRETRY - число попыток
        breq    noattempt
        st      X,TEMP_CNT
        rjmp    transmit_init           ; rjmp    noattempt - подработка в motor_tn
noattempt:
        ret

transmit_init:                          ; Инициализация передачи данных
        ldi     XL,low(tbuffer-1)
        ld      BYTE_CNT,X+
        ld      TRANSMIT,X+
        sbr     LINE_CSR,1<<DATA_SENDING
signal:                                 ; передача сигнала квитирования
        in      TEMP,TIMSK              ; Interrupt enable for transmission start
        sbr     TEMP,(1<<OCIE2)
        out     TIMSK,TEMP
        ret

;***********************************************************************
;               Functional subroutines
;***********************************************************************
exchange_initialization:                ; UART  initialization
        lds     TEMP,low(baud)
        tst     TEMP
        brpl    wi1
        sbi     UCSRA,U2X
        clr     TEMP
wi1:    out     UBRRL,TEMP
        ldi     TEMP,(1<<TXEN)|(1<<RXEN)|(1<<RXCIE)|(1<<CHR9)
        out     UCSRB,TEMP
                                        ; Timer2 - line timer
        lds     BOTTOM,tbottom
        lds     TEMP,tswitch            ; Set
	out 	OCR2,TEMP
        out     TCNT2,BOTTOM
        ldi     TEMP,(1<<CS22)
        out     TCCR2,TEMP              ; Set normal mode  with 1/64 prescaler
        ldi     TEMP,(1<<OCIE2)|(1<<TOIE2)
        out     TIFR,TEMP
        ldi     TEMP,(1<<TOIE2)
        out     TIMSK,TEMP

        ldi     TEMP,(1<<ISC01)         ; Interrupts INT0 at falling edge - line monitor
        out     MCUCR,TEMP
        ldi     TEMP,(1<<INT0)          ; Line monitor enable
        out     GIFR,TEMP
        out     GIMSK,TEMP
        clr     XH
        ret

work_initialization:
        ldi     TEMP,(1<<CS02)|(1<<CS00); Timer 0 set up (f/1024)
        out     TCCR0,TEMP
        out     TCNT0,YH
        ldi     TEMP,(1<<TOIE0)
        out     TIFR,TEMP
        in      TEMP,TIMSK
        sbr     TEMP,(1<<TOIE0)
        out     TIMSK,TEMP              ; Timer 0 overflow interrupt  enable

        ldi     TEMP,(1<<CS11)|(1<<CTC1)
        out     TCCR1B,TEMP             ; Timer 1 set up f/8 and clear on compare match
                                        ; SPI_MasterInit: Mode 0, f/4
        ldi     TEMP,(1<<MSTR)
        out     SPCR,TEMP

        lds     STATUS,mod_status
        clr     ABS_POSL
        clr     ABS_POSH
        dec     ABS_POSL
        dec     ABS_POSH                ;ABS_POS=-1
        ldi     COUNTL,1
        clr     COUNTH
        rcall   motion_init
        ret

load_speed:                             ; Tracking rate load
        sbrc    STATUS,MOTION
        rjmp    busy
        cli
        out     OCR1AH,AH
        out     OCR1AL,AL
        sei
        rjmp    yes

test_motion:
        sbrc    STATUS,MOTION
        rjmp    busy
        rjmp    yes

turn_on:
        sbrc    STATUS,MOTION           ;
        rjmp    busy
	sbrs	STATUS,WINDING_OFF
        rjmp    busy
	lds	TEMP,save_phases
	out	PORTC,TEMP
	cbr	STATUS,(1<<WINDING_OFF)
	rjmp	yes

turn_off:
        sbrc    STATUS,MOTION           ;
        rjmp    busy
	in	TEMP,PORTC
	sts	save_phases,TEMP
	sbr	STATUS,(1<<WINDING_OFF)
	sbr	TEMP,(1<<DIS1)|(1<<DIS2)
	out	PORTC,TEMP
	rjmp	yes

comm_sens_on:
        sbrc    STATUS,MOTION           ;
        rjmp    busy
        cbi     PORTD,LED_ON
        rjmp    yes

comm_sens_off:
        sbrc    STATUS,MOTION           ;
        rjmp    busy
        sbi     PORTD,LED_ON
        rjmp    yes

comm_left:                              ; To left stop motion start
        sbrc    STATUS,MOTION
        rjmp    busy
        ldi     COUNTL,low(-MAXSHIFT)
        ldi     COUNTH,high(-MAXSHIFT)
        rcall   motion_init
        rjmp    yes

comm_right:                            ; To right stop motion start
        sbrc    STATUS,MOTION
        rjmp    busy
        ldi     COUNTL,low(MAXSHIFT)
        ldi     COUNTH,high(MAXSHIFT)
        rcall   motion_init
        rjmp    yes

comm_shift_at:
        sbrc    STATUS,MOTION
        rjmp    busy
        ldi     ZL,relative
        ld      COUNTL,Z+
        ld      COUNTH,Z+
        rcall   motion_init
        rjmp    yes

motion_init:
        tst     COUNTH
        brpl    to_right
        sbrc    STATUS,LEFT
        rjmp    busy
        cbr     STATUS,(1<<FORWARD)
        com     COUNTL
        com     COUNTH
        adiw    COUNTL,1
        rjmp    to_left
to_right:
        sbrc    STATUS,RIGHT
        rjmp    busy
        sbr     STATUS,(1<<FORWARD)
to_left:
        cbi     PORTD,LED_ON
	sbrs	STATUS,WINDING_OFF
        rjmp    winding_on
	lds	TEMP,save_phases
	out	PORTC,TEMP
	cbr	STATUS,(1<<WINDING_OFF)
winding_on:
        ldi     ZL,relative
        st      Z+,COUNTL
        st      Z+,COUNTH
        cli
        sbr     STATUS,(1<<MOTION)
        out     TCNT1H,YH               ; Clear timer 1
        out     TCNT1L,YH
        lds     AL,speed
        lds     AH,speed+1
        out     OCR1AH,AH               ; Loading initial speed
        out     OCR1AL,AL
        ldi     TEMP,(1<<OCIE1A)
        out     TIFR,TEMP               ; Timer 1 flag  clear
        in      TEMP,TIMSK
        sbr     TEMP,(1<<OCIE1A)
        out     TIMSK,TEMP              ; Interrupt enable
        sei
        ret

stop_motion:                            ; Stop of a fast motion
        sbrs    STATUS,MOTION           ; ACW if no motion
        rjmp    busy
        rcall   do_stop
        rjmp    yes

do_stop:                                ; Emergency stop
        clr     COUNTL
        clr     COUNTH
        cli
        rcall   free_motion
        ret

comm_null:                              ; Absolute position clear
        ldi     TEMP,(4*MICRO-1)
        and     ABS_POSL,TEMP
        clr     ABS_POSH
        ldi     ZL,position
        st      Z+,ABS_POSL
        st      Z+,ABS_POSH
        rjmp    yes

constants:                              ; Module constants transmission
        ldi     ZL,low(apparatus*2)
        skip
identification:                         ; Module identification transmission
        ldi     ZL,low(identno*2)
        ldi     TEMP_CNT,5              ; Data length + 1
        ldi     YL,low(tnumber)
        st      Y+,TEMP_CNT             ; Block length
        dec     TEMP_CNT
        st      Y+,TEMP_CNT             ; Data header
id0:    lpm     TEMP,Z+                 ; From table to
        st      Y+,TEMP                 ; transmitter buffer copy
        dec     TEMP_CNT
        brne    id0
        st      Y+,YH                   ; 0 instead CRC
        rjmp    crc_gen

;***********************************************************************
;               General subroutines (do not use in interrupt handlers)
;***********************************************************************
sram_initialization:
                                        ; Clear 128 bytes of the SRAM
        ldi     YL, parameters
        clr     YH
        ldi     TEMP_CNT,0x80
cl1:    st      Y+,XH
        dec     TEMP_CNT
        brne    cl1
                                        ; Copy block from FLASH into SRAM
        ldi     ZL,low(line*2)
        clr     ZH
        ldi     TEMP_CNT,NLINE          ; Number of bytes
        ldi     YL,low(adress)
fl0:    lpm     TEMP,Z+
        st      Y+,TEMP
        dec     TEMP_CNT
        brne    fl0

        ldi     ZL,low(defaults*2)
        ldi     TEMP_CNT,2*(enddefaults-defaults) ; Number of bytes
        ldi     YL,low(speed)
fl1:    lpm     TEMP,Z+
        st      Y+,TEMP
        dec     TEMP_CNT
        brne    fl1
        ret

rr_copy:                                ; Copy block SRAM to SRAM
        ld      TEMP,Z+
        st      Y+,TEMP
        dec     TEMP_CNT
        brne    rr_copy
        ret

crc_check:                              ; CRC check
        ldi     YL,low(rbuffer-1)
        rcall   crc
        tst     AH
        breq    crc_ok
        ldi     TRANSMIT,NAK
        rcall   signal
        clz
crc_ok: ret

crc_gen:                                ; Add CRC while transmission
        ldi     TEMP,MAXRETRY
        st      Y,TEMP
        ldi     YL,low(tbuffer-1)
        rcall   transmit_init

crc:    ld      ARGUMENT,Y+             ; CRC computation
crcf:   ld      AH,Y+
        ldi     TEMP,DIVISOR
cr2:    ld      AL,Y+
        lsl     AL
        rol     AH
        brcc    PC+2
        eor     AH,TEMP                 ; CRC в старшем байте AH
        lsl     AL
        rol     AH
        brcc    PC+2
        eor     AH,TEMP
        lsl     AL
        rol     AH
        brcc    PC+2
        eor     AH,TEMP
        lsl     AL
        rol     AH
        brcc    PC+2
        eor     AH,TEMP
        lsl     AL
        rol     AH
        brcc    PC+2
        eor     AH,TEMP
        lsl     AL
        rol     AH
        brcc    PC+2
        eor     AH,TEMP
        lsl     AL
        rol     AH
        brcc    PC+2
        eor     AH,TEMP
        lsl     AL
        rol     AH
        brcc    PC+2
        eor     AH,TEMP
        dec     ARGUMENT
        brne    cr2
        st      -Y,AH
        ret

;***********************************************************************
;               SRAM organization
;***********************************************************************
.dseg
.org    0x60                    ; Parameters area (access via commands)
parameters:                     ; Initially loaded from EEPROM
mod_status:                     ; Copy of STATUS register for external access
        .byte   1
sens_status:                    ; Sensors current status
        .byte   1
position:
        .byte   2               ; Current motor position
relative:
        .byte   2               ; Relative free shift
speed:
        .byte   2               ; Tracking speed
key:    .byte   2               ; Key for program restart

.org    0x6F
ee_crc: .byte   1               ; CRC of EEPROM parameter block

.org    0x70                    ; Exchange paraneters, loaded from FLASH

adress: .byte   1               ; The module address
baud:   .byte   1               ; Exchange rate
tbottom:.byte   1               ; Line timer bottom value for WINDOW generation
tswitch:.byte   1               ; Transmittance switch delay

.org    0x78                    ; Variables area (access impossible)
left_chatter:
        .byte   1               ; Left sensor accumullator
right_chatter:
        .byte   1               ; Right sensor accumullator
save_phases:
	.byte	1		; Copy of port C

rnumber:.byte   1
rbuffer:.byte   19              ; Receiver buffer

tnumber:.byte   1
tbuffer:.byte   19              ; Transmitter buffer

.exit
