; кварц на 16 мгц ; внутренний делитель отключен .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<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 ; переход на обработку когда XY ;***************************************************************************************** ; ... ;***************************************************************************************** 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 ; выведем в порт сравнения длину импульса ; ( младщую часть hex числа ) ;************************ ; вычисляем 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 ; выведем в порт сравнения длину импульса ; ( младщую часть hex числа ) 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 ; выведем в порт сравнения длину импульса ; ( младщую часть hex числа ) ;************************ ; вычисляем 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 ; выведем в порт сравнения длину импульса ; ( младщую часть hex числа ) 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 ; флаг С - установлен когда XY ret xh_eq_yh: cp Temp1,YL ; сравниваем младшие части регистров ; флаг Z=1 когда X=Y ; флаг С - установлен когда XY 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 R16, YL ; копируем mov R17, YH ; 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 ;***************************************************************************************** ; ;*****************************************************************************************