; автор UWRTEY ( Екатеринбург ) // под строгим контролем Виталия Горбукова ака ВитГо
;
; кварц на 16 мгц
; внутренний делитель отключен
;
; Спасибо ВитГо ( Оренбург )
; Спасибо Леонид Иванович ( Минск )
; Спасибо zero648 ( Челябинская область )
; Спасибо Аlex ( Планета Земля )
;




.include 	"tn2313adef.inc"	; используем tiny2313a


.def	Temp=R16
.def	Temp1=R17
.def	Temp2=R18
.def	Temp3=R19
.def	Temp4=R20			; регистр исользуемый для сохранения/чтения регистра SREG
.def	Temp5=R21			; 
.def	Temp6=R22			; 
.def	Temp7=R23			; 
.def	Temp8=R24			;
.def	Temp9=R25			;










;******************************************************************************
; оперативка
;******************************************************************************



.dseg					; оперативка



ch_count: .byte 1			; номер импульса // 0,1,2 каналы
					; если = 3, то это синхроимпульс
					; если = 4, то это рассинхронизация или старт МК 


impuls_len: .byte 2			; зарезервируем 2 байта в ОЗУ = переменная для хранения данных считаных с таймера


THR_len: .byte 2			; зарезервируем 2 байта в ОЗУ = переменная для хранения данных "газ"


ruds_len: .byte 2			; зарезервируем 2 байта в ОЗУ = переменная для хранения данных "ruds"




;******************************************************************************
; константы
;******************************************************************************




.EQU	Chastota_const=0x9C40 		; вершина ШИМ для серопривода ( частота 50 Гц )
					; 50Гц=0,02сек
					; 1 тик таймера = 0,0000005 сек
					; 0,02/0,0000005=40000 ( 0x9C40 )



.equ	MIDDLE_RUD_const = 0x200	; 512 = нейтраль руля  ( летим прямо )



.EQU	const_1000 = 0x3E8 		; 1000 = число, для математических действий



;******************************************************************************
; программный сегмент // настройка прерываний
;******************************************************************************

.cseg

			

.org 0

rjmp RESET				; External Pin, Power-on Reset, Brown-out Reset, and Watchdog Reset ( включение или рестарт МК )






.org ICP1addr				; = 0x0003 Timer/Counter1 Capture Event ( прерывание по захвату )

rjmp ICP1_interruption




.org INT_VECTORS_SIZE




;******************************************************************************
; старт ***********************************************************************
;******************************************************************************

reset:					; включение микроконтроллера



	cli				; глобальный запрет на прерывания


	ldi Temp,low(RamEnd)		; инициализации стека	
	out SPL,Temp			;



;******************************************************************************
; настройка портов
;******************************************************************************

	
	ldi Temp,0b11111111		; настроили порт "В" (0-ввод, 1-вывод)
	out DDRB,Temp			; 000x0000 - вывод на сервопривод
					; 00000x00 - вывод на мотор
					 
		
	ldi Temp,0b00000000		; выводим лог_0
	out PortB,Temp			; 



		
												 
	ldi Temp,0b00111111		; настраиваем порт "D" (0-ввод, 1-вывод)
	out DDRD,Temp 			; 0х000000 - вход с приемника (ICP)
					; 00x00000 - вывод на мотор



	ldi Temp,0b00000000		; выводим лог_0
	out PortD,Temp			;



;	RCALL	LCD_INIT		; инициализация дисплея



;******************************************************************************
;  настраиваем таймер_0 
;******************************************************************************


; ШИМ для двигателей ( аппаратный )


	ldi Temp,(1<<COM0A1)|(1<<COM0B1)|(1<<wgm01)|(1<<wgm00)		; быстрый ШИМ с вершиной в 0xFF
	out TCCR0A,Temp							; очищать при совпадении, устанавливать при сбросе... 
								 

					
	ldi Temp,(1<<CS01)		; установили тактовый сигнал = CK 1:8 
	out TCCR0B,Temp			; таймер начал считать!


	Ldi Temp, 0			; зададим минимальное значение ШИМа на оба мотора
	OUT OCR0A, Temp			; выведем в порт сравнения длину импульса
	OUT OCR0B, Temp			; выведем в порт сравнения длину импульса


;******************************************************************************
;  настраиваем таймер_1
;******************************************************************************



; ШИМ для сервы ( аппаратный )
; ICP для измерения длительности импульса.


					

	ldi Temp,(1<<COM1A1)|(1<<COM1B1)|(1<<WGM11)|(1<<WGM10)
	out TCCR1A,Temp	
				
					; быстрый ШИМ с вершиной в OCR1A // очистить по совпадению, установить при сбросе
					; *1 ( ШИМ с регулируемой частотой )
					; генерации на OC1A в этом режиме нет! по идее можно отключить!


					; COM1A1  / поведение вывода OC1A // 
					; COM1A0  / поведение вывода OC1A // 
					; COM1B1  / поведение вывода OC1B // 
					; COM1B0  / поведение вывода OC1B //  
					; WGM11=1 / режим *
					; WGM10=0 / режим *





	ldi Temp,(0<<ICES1)|(1<<WGM13)|(1<<WGM12)|(1<<cs11)	
	out TCCR1B,Temp					
					; захват с ноги ICP по падающему фронту							;*****
					; тактовый сигнал таймера 1/8 ( 1 тик таймера = 0,5 мкс )
					; *1



					; тактовый сигнал = CK 1:8 (16МГц/8=2Мгц) 1 тик таймера = 0,5 мкс
					; таймер начал считать!!!

					; ICNC1=0 / фильтр шума на контакте ICP // отключено
					; ICES1=0 / захват с ноги ICP по падающему фронту
					; WGM13=1 / режим *
					; WGM12=1 / режим * Быстрый ШИМ ( Fast PWM )
					; CS12=0  / тактовый сигнал
					; CS11=1  / тактовый сигнал 
					; CS10=0  / тактовый сигнал = CK 1:8 (16МГц/8=2Мгц) 1 тик таймера = 0,5 мкс




	LDI Temp,high(Chastota_const)	; зададим Частоту ШИМ ( установим вершину ШИМ )
	OUT OCR1AH,Temp			; и выведем ее в порт сравнения
	LDI Temp,low(Chastota_const)							
	OUT OCR1AL,Temp	



	ldi xh, 0xb			; 3000 = 0x0BB8 - крайнее положение сервопривода... ( весь диапазон от 2000 до 4000 )
	ldi xl, 0xb8


	OUT OCR1BH,XH			; зададим длину импульса ( среднее положение )
	OUT OCR1BL,XL			; и выведем ее в порт сравнения





	ldi Temp,(1<<ICIE1)		;  прерывание по захвату включено
	out TIMSK,Temp

					; TOIE1=0  / прерывание по переполнению счетчика таймера_1 // отключено
					; OCIE1A=0 / прерывание компатора_A таймера_1 // отключено 
					; OCIE1B=0 / прерывание компатора_B таймера_1 // отключено
					; ICIE1=1  / прерывание по захвату // включено
					; OCIE0B=0 / прерывание компатора_B таймера_0 // отключено
					; TOIE=0   / прерывание по переполнению счетчика таймера_0 // отключено
					; OCIE0A=0 / прерывание компатора_A таймера_0 // отключено





;******************************************************************************
; настраиваем переменные
;******************************************************************************


	ldi Temp, 4			; устанавливаем флаг рассинхронизации/включения_мк
	sts ch_count, Temp






;******************************************************************************
; сброс таймеров
;******************************************************************************



	ldi Temp,0			; обнуление таймера_0
	out TCNT0,Temp



	ldi Temp,0
	STS impuls_len, Temp		; обнуляем данные таймера в оперативке
	STS impuls_len+1, Temp		; 



	ldi Temp,0			; обнуление таймера_1
	out TCNT1H,Temp
	out TCNT1L,Temp



	sei				; глобальное разрешение прерываний




	

;*******************************************************************************
; главный цикл *****************************************************************
;*******************************************************************************


Main_cicle:


	
	rjmp Main_cicle			; циклимся






;*******************************************************************************
; обработка прерывания *********************************************************
;*******************************************************************************

ICP1_interruption:
	



	in temp4,SREG			; сохраняем "SREG"/////////////////////////////////////////////////////
	push temp4




;*******************************************************************************


	in XL,ICR1L 			; читаем младший байт счетчика
	in XH,ICR1H 			; читаем старший байт счетчика



;**************************************************************************************************************
; вычисление длины импульса/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\
;**************************************************************************************************************


	lds ZL, impuls_len		; загружаем (читаем )  в "z" данные таймера с прошлого раза 
	lds ZH, impuls_len+1


	STS impuls_len, XL		; сохраняем данные счетчика полученные в этот раз
	STS impuls_len+1, XH		; 



	mov YH,ZH			; копируем  данные таймера с прошлого раза 
	mov YL,ZL


	mov Temp,XH			; копируем данные таймера полученные сейчас
	mov Temp1,XL

	rcall compare_XY16		; сравниваем...


	breq menshe_2			; переход на обработку когда X=z ( сейчас число равно тому, что было в прошлый раз )
	brcs menshe_2			; переход на обработку когда X<z ( сейчас число меньше чем в прошлый раз )
	rjmp bolshe_2			; переход на обработку когда X>z ( сейчас число больше чем в прошлый раз )


;**************************************************************************************************************
; вычисление длины импульса, если таймер был переполнен
;**************************************************************************************************************

menshe_2:



	SUB ZL, XL			; из большего вычитаем меньшее
	SBC ZH, XH			;

	ldi XH, high(Chastota_const)	; загружаем число 40000
	ldi XL, low(Chastota_const)

	SUB XL, ZL			; вычисляем длительность импульса
	SBC XH, ZH			; длительность канала находится в "Х"

	rjmp prodolzaem			; 

;**************************************************************************************************************
; вычисление длины импульса, если таймер НЕ БЫЛ переполнен
;**************************************************************************************************************

bolshe_2:


	SUB XL, ZL			; вычисляем длительность импульса
	SBC XH, ZH			; длительность канала находится в "Х"

	rjmp prodolzaem			; 

;**************************************************************************************************************
; проверка синхронизации ( а так-же нужно при включении мк )
;**************************************************************************************************************

prodolzaem:


	LDS Temp, ch_count		; читаем номер импульса


	cpi Temp, 4			; флаг рассинхронизации установлен ?
	brne work			; флаг снят, Все сихронизировано!!!!
	rjmp sinhro			; флаг установлен, требуется синхронизация





;*********************************************************************************
; сравниваем полученную длительность с 13 мс ( 0b110010110010000 ) ( 0x6590 )
;*********************************************************************************

sinhro:

	mov Temp,XH			; копируем  "Х"
	mov Temp1,XL



	ldi YH,0x65			; записываем в регистровую пару "Y" 13мс  0x6590
	ldi YL,0x90



	rcall compare_XY16		; сравниваем...

	breq bolshe			; переход на обработку когда X=Y 
	brcs menshe			; переход на обработку когда X<Y
	rjmp bolshe			; переход на обработку когда X>Y 




;*****************************************************************************************
; ...
;*****************************************************************************************




bolshe:					; это синхроимпульс! 


	ldi Temp, 0			; снимаем флаг включения_МК, устанавливаем флаг 0-го канала 
	sts ch_count, Temp

	rjmp out_1			; выход






menshe:					; это был не синхроимпульс!

	rjmp out_1			; выход







;*****************************************************************************************
; все синхронизировано, можно работать
;*****************************************************************************************


work:						;



channal_check_0:				; проверка канала_0


	LDS Temp, ch_count			; читаем номер импульса


	cpi Temp, 0				; это 0-ой канал ?
	brne channal_check_1			; нет
	rjmp channal_0				; да






channal_check_1:				; проверка канала_1


	LDS Temp, ch_count			; читаем номер импульса


	cpi Temp, 1				; это 1-ый канал ?
	brne channal_2_A			; нет, это канал_2
	rjmp channal_1				; да, это канал_1


channal_2_A:					; вне зоны доступа ( метка слишком далеко ) возможно придется переносит в главный цикл...

	rjmp channal_2				; нет, это канал_2

;*****************************************************************************************
; РАБОТА С КАНАЛАМИ
;*****************************************************************************************




channal_0:					; канал_0 ( газ )

	
	ldi Temp, 1				; следующим будет канал_1
	sts ch_count, Temp




	rcall calculation_of_ADC		; вычисляем  значение THR из длительности таймера




	STS THR_len, XL				; сохраняем в переменную "THR_len" значение THR
	STS THR_len+1, XH			; 


	rjmp out_1				; выход	




;**********************************************************************************************
;**********************************************************************************************


channal_1:					; канал_1 ( курс )




	ldi Temp, 2				; следующим будет канал_2
	sts ch_count, Temp




	rcall calculation_of_ADC		; вычисляем  значение RUD из длительности таймера






; попытка микширования каналов ;) ____________________________________________________________


; RUDS=RUD-MIDDLE_RUD

	ldi Temp, Low(MIDDLE_RUD_CONST)		; загружаем число 512
	ldi Temp1, high(MIDDLE_RUD_CONST)


	SUB XL, Temp				; вычисляем RUDS ( из RUD вычитаем MIDDLE_RUD )
	SBC XH, Temp1				;



	STS RUDS_len, XL			; сохраняем в переменную "RUDS_len" значение RUDS
	STS RUDS_len+1, XH			; 




; проверяем результат на отрицательность

	mov Temp, XH				; копируем XH в Temp

	andi Temp, 0b10000000			; маска ( где 1, там остается информпция, где 0, так стирается )

	cpi Temp, 0b10000000			; это число отрицательное ?
	brne to_the_right			; нет 
	rjmp to_the_left			; да









;_______________________________________________________________________________________________
;_______________________________________________________________________________________________

to_the_right:					; курс вправо ( положительное число )

; формулы
; THR1=THR+RUDS
; THR2=THR-RUDS





	lds XL, RUDS_len			; читаем значение RUDS 
	lds XH, RUDS_len+1



;************************
; вычисляем THR1=THR+RUDS

	lds YL, THR_len				; читаем значение THR
	lds YH, THR_len+1	



	add YL, XL				; THR+RUDS
	adc YH, XH				;


; делим на шесть

	rcall div16to2

; отдаем команду правому мотору ( Мотор "А" )



	OUT OCR0A, R18				; выведем в порт сравнения длину импульса						



;************************
; вычисляем THR1=THR-RUDS

	lds YL, THR_len				; читаем значение THR
	lds YH, THR_len+1	



	SUB YL, XL				; THR-RUDS
	SBC YH, XH				;


; делим на шесть

	rcall div16to2

; отдаем команду левому мотору ( Мотор "Б" )



	OUT OCR0B, R18				; выведем в порт сравнения длину импульса 						



	rjmp out_1				; выход






;_______________________________________________________________________________________________
;_______________________________________________________________________________________________

to_the_left:					; курс влево ( отрицательное число )

; формулы
; THR1=THR-RUDS
; THR2=THR+RUDS 


; превратим отрицательное число в положительное 


; 0-(-RUDS)=RUDS

	ldi Temp, 0
	ldi Temp1, 0


	lds XL, RUDS_len			; читаем значение RUDS
	lds XH, RUDS_len+1



	SUB Temp, XL				; превращаем отрицательное число в положительное 
	SBC Temp1, XH				; результат в Temp и Temp1



	mov  XL, Temp				; далее будем работать с регистровой парой и поэтому помещаем результат в нее
	mov  XH, Temp1





;************************
; вычисляем THR1=THR-RUDS

	lds YL, THR_len				; читаем значение THR
	lds YH, THR_len+1	


	sub YL, XL				; THR-RUDS
	sbc YH, XH				;


; делим на шесть

	rcall div16to2

; отдаем команду правому мотору ( Мотор "А" )



	OUT OCR0A, R18				; выведем в порт сравнения длину импульса 						





;************************
; вычисляем THR1=THR+RUDS

	lds YL, THR_len				; читаем значение THR
	lds YH, THR_len+1	


	add YL, XL				; THR+RUDS
	adc YH, XH				;


; делим на шесть

	rcall div16to2

; отдаем команду левому мотору ( Мотор "Б" )



	OUT OCR0B, R18				; выведем в порт сравнения длину импульса 						


	rjmp out_1				; выход





;**********************************************************************************************
;**********************************************************************************************

channal_2:					; канал_2 ( сервопривод тангажа )





	ldi Temp, 4				; следующим будет СИНХРОИМПУЛЬС
	sts ch_count, Temp





						 
	OUT OCR1BH,XH				; зададим длину импульса
	OUT OCR1BL,XL				; и выведем ее в порт сравнения




	rjmp out_1				; выход

;no_out:

;	rjmp no_out				; зависание





;**************************************************************************************************************
; Выход из прерывания
;**************************************************************************************************************

out_1:	

	

	pop temp4			; извлекаем "SREG" ////////////////////////////////////////////////////
	out SREG, temp4

	RETI				; точка выхода из прерывания



;*****************************************************************************************
; подпрограмма сравнения 
;*****************************************************************************************


compare_XY16:

	cp Temp,YH			; сравниваем старшие части регистров
	breq xh_eq_yh			; значения старших регистров равны, нужно сравнить младшие xl и yl
					; флаг С - установлен когда X<Y
					; флаг С - сброшен когда X>Y
	ret



xh_eq_yh:

	cp Temp1,YL			; сравниваем младшие части регистров
					; флаг Z=1 когда X=Y
					; флаг С - установлен когда X<Y
					; флаг С - сброшен когда X>Y

	ret                            


;*****************************************************************************************************************
; подпрограмма вычисления значения АЦП из длительности таймера ( x/2-1000 )
;*****************************************************************************************************************

calculation_of_ADC:

; /2 ( разделить на два )



	LSR XL					; логический сдвиг вправо 16-ти битного числа ( разделить на два )
	ROR XH					; ( циклический сдвиг вправо )


; -1000

	ldi Temp, Low(const_1000)
	ldi Temp1, high(const_1000)


	SUB XL, Temp				; -1000 
	SBC XH, Temp1				;

	ret					; выход из подпрограммы


;*****************************************************************************************************************
; подпрограмма деления на шесть ( делим сначала на 2 а потом еще на 3 )
;*****************************************************************************************************************

div16to2:			; делим на 2

	LSR YL			; логический сдвиг вправо
	ROR YH			; циклический сдвиг вправо

	mov YL, R16		; копируем
	mov YH, R17


; R19:R18 = R17:R16/3
; R19:R18 – частное
; R17:R16 – делимое
; R20 – вспомогательный регистр
  
div16to3:			; делим на 3

	clr R18			; очищаем вспомогательные регистры R18,R19
	clr R19			; при входе в подпрограмму  

du1:

	rcall shift_right

	brne PC+2		; если Z=1,
	ret			; то завершаем деление

	add R18,R16		; в ином случае добавляем к накопителю
	adc R19,R17		; очередной нечётный член ряда

	rcall shift_right

	brne PC+2		; если Z=1,
	ret			; то завершаем деление

	sub R18,R16		; в ином случае вычитаем из накопителя
	sbc R19,R17		; очередной чётный член ряда

	rjmp  du1
 
shift_right:

	lsr R17			; производим деление R17:R16 / 2,
	ror R16			; получая очередной член ряда
	mov R20,R17		; если R20 = R17+R16 = 0 (т.е. R17:R16=0),
	or R20,R16		; то выходим из подпрограммы с флагом Z=1
	ret 







;*****************************************************************************************
; 
;*****************************************************************************************