;#########################
;###       CA-722      ###
;#########################
;Version	 2.1
;Date:		 07 JUN 2005
;Filename:	 CA-722.ASM
;Support E-mail: info@astroma-spb.ru
;Target:	 ACM-801, ACM-811

;ПРОГРАММА УПРАВЛЕНИЯ ЗАЧИСТНЫМ СТАНКОМ CA-722 ФИРМЫ MURAT

.INCLUDE "m64def.inc"	;файл предоставляется компанией Atmel (www.atmel.com)
.INCLUDE "ACM-801.inc"	;файл предоставляется компанией Astroma Ltd. (www.astroma-spb.ru)

.equ	SRAM_ADDRESS	=$4000	;начало рабочей области в SRAM

		.CSEG
BEGIN:		CALL	GetBaseAddress		;определить базовый адрес программы
		mov	yl,xl			;сохранить в регистровой паре Y
		mov	yh,xh
;подключение обработчика задачи управления
		ldi	r25,HOOK_SYS_TIME	;идентификатор ловушки
		CALL	GetHookHandler		;получить адрес обработчика ловушки
		sts	(RET_ADDRESS),xl	;сохранить для возврата
		sts	(RET_ADDRESS+1),xh
		clr	xl			;инициализация переменных
		ldi	xh,Clr_Len
		ldi	zl,low(Clr_Begin)
		ldi	zh,high(Clr_Begin)
CLR_VAR:	st	Z+,xl
		dec	xh
		brne	CLR_VAR
		ldi	xl,2
		sts	(TIME_DIV),xl
		ldi	xl,100
		sts	(SEC_DIV),xl
		ldi	xl,low(TASK)		;X - смещение обработчика задачи управления
		ldi	xh,high(TASK)
		add	xl,yl			;прибавить базовый адрес
		adc	xh,yh			;X - адрес обработчика задачи управления
		ldi	r25,HOOK_SYS_TIME
		CALL	SetHook			;настроить ловушку 200 Hz
;подключение обработчика команды 05h
		ldi	xl,low(COMM_05h)
		ldi	xh,high(COMM_05h)
		add	xl,yl
		adc	xh,yh
		ldi	r25,$05			;код команды
		CALL	SetCmdHandler		;установить адрес обработчика команды 02h
;подключение обработчика команды 06h
		ldi	xl,low(COMM_06h)
		ldi	xh,high(COMM_06h)
		add	xl,yl
		adc	xh,yh
		ldi	r25,$06			;код команды
		CALL	SetCmdHandler		;установить адрес обработчика команды 02h
		ret
;***************************************************************************
;ОБРАБОТЧИК ЗАДАЧИ УПРАВЛЕНИЯ
TASK:		push	r25
		lds	r25,(TIME_DIV)	;делитель частоты (получение 100Hz)
		dec	r25
		breq	RUN_TASK	;если 0 - перейти на обработчик задачи
		sts	(TIME_DIV),r25
;возврат в систему
TASK_EXIT:	lds	zl,(RET_ADDRESS)
		lds	zh,(RET_ADDRESS+1); Z - адрес возврата
		pop	r25
		ijmp
;---------------------------------------------------------------------------
RUN_TASK:	ldi	r25,2
		sts	(TIME_DIV),r25	;инициализция делителя
		lds	r25,(SEC_DIV)	;делитель для получения 1 Hz
		dec	r25
		brne	TIMER_COMPLETE
		ldi	zl,low(WORK_TIME)
		ldi	zh,high(WORK_TIME)
		rcall	INC_DWORD
		ldi	r25,100
TIMER_COMPLETE:	sts	(SEC_DIV),r25
		sbic	PIND,3		;проверка блокировок (Digital IN 3)
		rjmp	TASK_CONTINUE
		lds	r25,(TASK_PHASE)
		or	r25,r25
		breq	TASK_EXIT
;остановка при срабатывании блокировок
TASK_STOP:	sts	(TASK_WAIT),r25	;сброс паузы
		sts	(TASK_WAIT+1),r25
		in	r25,PORTB
		andi	r25,$F0		;выключить все
		out	PORTB,r25
TASK_RESTART:	clr	r25		;сбросить выполнение задачи
		sts	(TASK_PHASE),r25
		rjmp	TASK_EXIT
;--------------------------------------------------------------------------
;отсчет устанавливаемых пауз
TASK_PAUSE:	lds	zl,(TASK_WAIT)
		lds	zh,(TASK_WAIT+1)
		or	zl,zh
		breq	TASK_PAUSE_END
		lds	zl,(TASK_WAIT)
		sbiw	zh:zl,1
		sts	(TASK_WAIT),zl
		sts	(TASK_WAIT+1),zh
TASK_PAUSE_END:	ret
;--------------------------------------------------------------------------
;выбор фазы выполнения задачи
TASK_CONTINUE:	lds	r25,(TASK_PHASE)
		or	r25,r25
		breq	TASK_PHASE_0
		dec	r25
		breq	TASK_PHASE_1
		dec	r25
		breq	TASK_PHASE_2
		dec	r25
		brne	TASK_CONT_0
		rjmp	TASK_PHASE_3
TASK_CONT_0:	dec	r25
		brne	TASK_CONT_1
		rjmp	TASK_PHASE_4
TASK_CONT_1:	dec	r25
		brne	TASK_STOP
		rjmp	TASK_PHASE_5
;ФАЗА 0 - ожидание установки профиля
TASK_PHASE_0:	rcall	TASK_PAUSE	;если Z устанолен, пауза завершена
		brne	TASK_EXIT_X
		sbis	PIND,0		;проверка наличия профиля
		rjmp	TASK_EXIT
		ldi	r25,25		;устновить паузу 0.25с
		sts	(TASK_WAIT),r25
		ldi	r25,1
		sts	(TASK_PHASE),r25;установка фазы 1		
TASK_EXIT_X:	rjmp	TASK_EXIT
;ФАЗА 1 - прижим профиля
TASK_PHASE_1:	rcall	TASK_PAUSE	;если Z устанолен, пауза завершена
		brne	TASK_EXIT_X
		sbis	PIND,0		;проверка наличия профиля
TASK_STOP_X:	rjmp	TASK_STOP
		sbi	PORTB,3		;запуск прижима
		ldi	r25,$01		;установить паузу 3.5c  ( 015Eh )
		sts	(TASK_WAIT+1),r25;(время срабатывания прижима)
		ldi	r25,$5E
		sts	(TASK_WAIT),r25
		ldi	r25,2		;установить фазу 2
		sts	(TASK_PHASE),r25
		rjmp	TASK_EXIT
;ФАЗА 2 - запуск ножей
TASK_PHASE_2:	sbis	PIND,0		;проверка наличия профиля
		rjmp	TASK_STOP
		rcall	TASK_PAUSE	;если Z устанолен, пауза завершена
		brne	TASK_EXIT_X
;начиная с этого момента возврат в фазу 0 только через паузу 2c
		sbic	PIND,4		;проверка S0
		rjmp	RUN_PHASE_3	;если только фреза, пропустить ножи
		sbic	PIND,2		;проверка положения фрезы (S6)
		rjmp	TASK_COMPLETE
		sbi	PORTB,2;	;пуск ножей
		ldi	r25,3		;устаноить фазу 3	
		sts	(TASK_PHASE),r25
		rjmp	TASK_EXIT
;ФАЗА 3 - запуск фрезы
TASK_PHASE_3:	sbis	PIND,1		;проверить S5
		rjmp	TASK_EXIT	;ножи не дошли, выход	
RUN_PHASE_3:	sbic	PIND,5		;проверка S1
		rjmp	TASK_PHASE_4_1	;если только ножи, пропустить фрезу
		cbi	PORTB,2		;отключение ножей
		sbi	PORTB,0		;пуск двигателя фрезы
		sbi	PORTB,1		;пуск подачи фрезы
		ldi	r25,4		;установить фазу 4
		sts	(TASK_PHASE),r25
		ldi	r25,40		;пауза 0.4с
		sts	(TASK_WAIT),r25
		rjmp	TASK_EXIT
;ФАЗА 4 - завершение процесса обработки
TASK_PHASE_4:	rcall	TASK_PAUSE
		brne	TASK_EXIT_X
		in	r25,PIND
		andi	r25,0b00000110	;если 0 - идет процесс обработки
		breq	TASK_EXIT_X
TASK_PHASE_4_1:	in	r25,PORTB
		andi	r25,$F0		;выключить все
		out	PORTB,r25
		ldi	r25,5		;установить фазу 5
		sts	(TASK_PHASE),r25
		rjmp	TASK_EXIT
;ФАЗА 5 - ожидание снятия профиля	
TASK_PHASE_5:	sbic	PIND,0		;проверка наличия профиля
		rjmp	TASK_EXIT	;профиль не снят
TASK_COMPLETE:	ldi	r25,200		;иначе установить паузу 2с	
		sts	(TASK_WAIT),r25
;подсчет операций по видам обработки
		in	r25,PIND
		lsr	r25
		lsr	r25
		andi	r25,$0C
		cpi	r25,$0C
		brcc	COUNTER_END		
		ldi	zl,low(OP0_COUNTER)
		ldi	zh,high(OP0_COUNTER)
		add	zl,r25
		ldi	r25,$00
		adc	zh,r25
		rcall	INC_DWORD
COUNTER_END:	rjmp	TASK_RESTART
;****************************************************
INC_DWORD:	push	zl
		push	zh
		rcall	INC_WORD
		brcc	INC_DWORD_COMP
		rcall	INC_WORD
INC_DWORD_COMP:	pop	zh
		pop	zl
		ret
;----------------------------------
INC_WORD:	push	xl
		push	xh
		ld	xl,Z
		ldd	xh,Z+1
		adiw	xh:xl,1
		st	Z+,xl
		st	Z+,xh
		pop	xh
		pop	xl
		ret
;****************************************************
;обработчик команды 05h - чтение счетчиков
COMM_05h:	push	xl
		push	xh
		CALL	GetSysReceiveBuffer	;получить адрес приемного буфера по UART
		adiw	xh:xl,1			;указать на длину пакета
		ld	r25,X
		cpi	r25,3			;проверить длину
		brne	COMM_ERROR
		adiw	xh:xl,2			;указать OpCode
		ld	r25,X
		cpi	r25,3			;проверить OpCode
		brcc	COMM_ERROR
		lsl	r25
		lsl	r25
		push	r24
		push	r25
;ФОРМИРОВАНИЕ ОТВЕТНОГО ПАКЕТА
		CALL	GetSysSendBuffer	;получить адрес буфера передачи
		ldi	r25,10			;длина ответного пакета
		st	X+,r25
		ldi	r25,$06			;ACK
		st	X+,r25
;копирование счетчика времени
		ldi	zl,low(WORK_TIME)
		ldi	zh,high(WORK_TIME)
		ldi	r24,4			;длина счетчика
COMM_05h_1:	ld	r25,Z+
		st	X+,r25
		dec	r24
		brne	COMM_05h_1
		pop	r25
;копирование счетчика указанной операции
		add	zl,r25
		adc	zh,r24
		ldi	r24,4			;длина счетчика
COMM_05h_2:	ld	r25,Z+
		st	X+,r25
		dec	r24
		brne	COMM_05h_2
		pop	r24
COMM_05h_EXIT:	pop	xh
		pop	xl
		ret
;****************************************************
;ФОРМИРОВАНИЕ ОТВЕТНОГО ПАКЕТА
;[Len][15h][ChSum]
COMM_ERROR:	CALL	GetSysSendBuffer	;получить адрес буфера передачи
		ldi	r25,$02			;Len
		st	X+,r25
		ldi	r25,$15			;NACK
		st	X+,r25
		rjmp	COMM_05h_EXIT
;****************************************************
;обработчик команды 06h - сбросить счетчики
COMM_06h:	push	xl
		push	xh
		CALL	GetSysReceiveBuffer	;получить адрес приемного буфера по UART
		adiw	xh:xl,1			;указать на длину пакета
		ld	r25,X
		cpi	r25,2			;проверить длину
		brne	COMM_ERROR
;ВЫПОЛНЕНИЕ КОМАНДЫ
		push	r24
		clr	r25
		ldi	xl,low(WORK_TIME)
		ldi	xh,high(WORK_TIME)
		ldi	r24,(Clr_End-WORK_TIME)
COMM_06h_1:	st	X+,r25
		dec	r24
		brne	COMM_06h_1
		pop	r24
;ФОРМИРОВАНИЕ ОТВЕТНОГО ПАКЕТА [Length] [06h] [ChSum]
		CALL	GetSysSendBuffer	;получить адрес буфера передачи
		ldi	r25,2			;длина ответного пакета
		st	X+,r25
		ldi	r25,$06			;ACK
		st	X+,r25
		pop	xh
		pop	xl
		ret
;****************************************************

		.DSEG
		.org	SRAM_ADDRESS

RET_ADDRESS:	.byte	2	;адрес возврата из обработчика CA-722
TIME_DIV:	.byte	1	;делитель частоты в 100 Hz
SEC_DIV:	.byte	1	;делитель частоты в 1 Hz

Clr_Begin:
TASK_WAIT:	.byte	2	;счетчик паузы
TASK_PHASE:	.byte	1	;фаза выполнения задачи
;переменные для ведения учета
WORK_TIME:	.byte	4	;время работы (сек)
OP0_COUNTER:	.byte	4	;счетчик операций 0
OP1_COUNTER:	.byte	4	;счетчик операций 1
OP2_COUNTER:	.byte	4	;счетчик операций 2
Clr_End:
	.equ	Clr_Len=Clr_End-Clr_Begin