Подпрограмма прерывания от таймера/счетчика 3
С учетом того, что для отсчета угла используется один байт (восемь разрядов), введен коэффициент 0,7 для получения кода угла:
0º 0
240º 84
120º 168
360º 252
Подпрограмма расчета угла очередной фазы
Если угол текущей фазы менее 240 градусов, то угол очередной фазы определяется путем увеличения исходного угла на 120 градусов, в противном случае из него вычитается 240 градусов.
Подпрограмма выбора скважности для заданного угла
Подпрограмма определяет, в каком из четырех квадрантов находится заданный угол, и в зависимости от этого корректирует угол (до значения, не превышающего 90 градусов) и извлекает из таблицы соответствующее значение скважности. Также устанавливается признак полярности – разряды выбора ключей.
.include "m64def.inc" ;Подключение библиотеки микроконтроллера ATmega64
;*** Определение рабочих регистров
.def Segm_Reg =R16 ;Регистр для хранения угла
.def PWM_Reg =R17 ;Регистр для хранения скважности
.def Key_Reg =R18 ;Регистр для хранения выборки ключей
.def Tmp1_Reg =R19 ;Регистр 1 для временного хранения данных
.def Tmp2_Reg =R20 ;Регистр 2 для временного хранения данных
;*** Определение адресов ячеек SRAM
.equ SRAl =0x0062 ;Признак направления вращения
.equ SysRegINl =0x0064 ;Младший байт регистра состояния (считанный)
.equ SysRegINh =0x0065 ;Старший байт регистра состояния (считанный)
.equ SysRegOUTl =0x0066 ;Младший байт регистра состояния (для записи)
.equ SysRegOUTh =0x0067 ;Старший байт регистра состояния (для записи)
.equ DataRegINl =0x0068 ;Младший байт регистра данных (считанный)
.equ DataRegINh =0x0069 ;Старший байт регистра данных (считанный)
.equ DataRegOUTl =0x006A ;Младший байт регистра данных (для записи)
.equ DataRegOUTh =0x006B ;Старший байт регистра данных (для записи)
.equ SetDataR1 =0x0070 ;Заданное значение регулятора 1 (X[n])
.equ InDataR1 =0x0071 ;Значение обратной связи регулятора 1 (Xос[n])
.equ DelDataR1 =0x0072 ;Значение рассогласования регулятора 1 (dX[n])
.equ Del1DataR1 =0x0073 ;Прошлое значение рассогласования регулятора 1 (dX[n-1])
.equ MinDelDataR1 =0x0074 ;Минимальное значение рассогласования регулятора 1 (Xmin)
.equ MaxDelDataR1 =0x0075 ;Максимальное значение рассогласования регулятора 1 (Xmax)
.equ B0_F_R1 =0x0076 ;Дробная часть коэффициента b0 регулятора 1
.equ B0_W_R1 =0x0077 ;Целая часть коэффициента b0 регулятора 1
.equ B1_F_R1 =0x0078 ;Дробная часть коэффициента b1 регулятора 1
.equ B1_W_R1 =0x0079 ;Целая часть коэффициента b1 регулятора 1
.equ OutDataR1 =0x007A ;Выходное значение регулятора 1 (Y[n])
.equ Out1DataR1l =0x007B ;Дробная часть прошлого выходного значения регулятора 1 (Y[n-1])
.equ Out1DataR1h =0x0090 ;Целая часть прошлого выходного значения регулятора 1 (Y[n-1])
.equ MinOutDataR1 =0x007D ;Минимальное выходное значение регулятора 1 (Ymin)
.equ MaxOutDataR1 =0x007E ;Максимальное выходное значение регулятора 1 (Ymax)
;*** Определение разрядов
.equ TabStartl =0x00 ;Начало таблицы в EEPROM
.equ TabStarth =0x00 ;Начало таблицы в EEPROM
;*** Определение постоянных величин
.equ Ang0 =0 ;Угол 0 градусов
.equ Ang30 =21 ;Угол 30 градусов
.equ Ang90 =63 ;Угол 90 градусов
.equ Ang120 =84 ;Угол 120 градусов
.equ Ang150 =105 ;Угол 150 градусов
.equ Ang180 =126 ;Угол 180 градусов
.equ Ang210 =147 ;Угол 210 градусов
.equ Ang240 =168 ;Угол 240 градусов
.equ Ang270 =189 ;Угол 270 градусов
.equ Ang330 =231 ;Угол 330 градусов
.equ Ang360 =252 ;Угол 360 градусов
.equ AngMax =251 ;Максимальный угол (358,57 градуса)
.equ AngStep =1 ;Шаг приращения угла (1,43 градуса)
.equ StopKey =0b00 ;Код отсутствия выбора ключей фазы
.equ PlusKey =0b01 ;Код выбора положительного ключа (верхнего)
.equ MinusKey =0b10 ;Код выбора отрицательного ключа (нижнего)
;*** Вектора прерываний
rjmp StartPoint ;Reset
nop ;INT0
nop ;INT1
rjmp IntSysReg ;INT2
nop ;INT3
nop ;INT4
nop ;INT5
nop ;INT6
nop ;INT7
nop ;TIMER2 COMP
nop ;TIMER2 OVF
nop ;TIMER1 CAPT
nop ;TIMER1 COMPA
nop ;TIMER1 COMPB
nop ;TIMER1 OVF
nop ;TIMER0 COMP
nop ;TIMER0 OVF
nop ;SPI, STC
nop ;USART0, RX
nop ;USART0, UDRE
nop ;USART0, TX
nop ;ADC
nop ;EE READY
nop ;ANALOG COMP
nop ;TIMER1 COMPC
nop ;TIMER3 CAPT
nop ;TIMER3 COMPA
nop ;TIMER3 COMPB
nop ;TIMER3 COMPC
nop ;TIMER3 OVF
nop ;USART1, RX
nop ;USART1, UDRE
nop ;USART1, TX
nop ;TWI
nop ;SPM READY
;*** Подпрограмма ПИ-регулятора
PIsub1:
push r0
push r1
push r16
push r17
push r18
push r19
push r20 ;Сохранение регистров в стеке
;Формирование рассогласования
lds r16,SetDataR1 ;Загрузка заданного значения
lds r17,InDataR1 ;Загрузка значения обратной связи (X[n])
sub r16,r17 ;Формирование рассогласования (dX[n])
;Сравнение рассогласования с минимальным значением
lds r17,MinDelDataR1 ;Загрузка минимального значения рассогласования регулятора (Xmin)
tst r17 ;Проверка знака Xmin
brmi PiLoop01 ;Переход если отрицательное
tst r16 ;Проверка знака dX[n]
brmi PiLoop02 ;Переход если отрицательное (dX[n] меньше Xmin)
cp r16,r17 ;Сравнение dX[n] и Xmin
brpl PiLoop03 ;Переход если dX[n] больше Xmin
PiLoop02:
mov r16,r17 ;Загрузка Xmin в dX[n]
rjmp PiLoop03 ;Переход дальше
PiLoop01:
tst r16 ;Проверка знака dX[n]
brpl PiLoop03 ;Переход если положительное (dX[n] больше Xmin)
cp r16,r17 ;Сравнение dX[n] и Xmin
brmi PiLoop02 ;Переход если dX[n] меньше Xmin
;Сравнение рассогласования с максимальным значением
PiLoop03:
lds r17,MaxDelDataR1 ;Загрузка максимального значения рассогласования регулятора (Xmax)
tst r17 ;Проверка знака Xmax
brmi PiLoop04 ;Переход если отрицательное
tst r16 ;Проверка знака dX[n]
brmi PiLoop06 ;Переход если отрицательное (dX[n] меньше Xmax)
cp r17,r16 ;Сравнение Xmax и dX[n]
brpl PiLoop06 ;Переход если dX[n] меньше Xmax
PiLoop05:
mov r16,r17 ;Загрузка Xmax в dX[n]
rjmp PiLoop06 ;Переход дальше
PiLoop04:
tst r16 ;Проверка знака dX[n]
brpl PiLoop05 ;Переход если положительное (dX[n] больше Xmax)
cp r16,r17 ;Сравнение dX[n] и Xmin
brmi PiLoop05 ;Переход если dX[n] меньше Xmin
;Вычисление выходного значения регулятора
PiLoop06:
lds r20,B0_F_R1 ;Загрузка дробной части b0 в дробную часть множимого
mul r16,r20 ;Перемножение дробной части b0 и dX[n] без знака
mov r18,r0 ;Загрузка младшего байта результата в Y[n]
mov r19,r1 ;Загрузка старшего байта результата в Y[n]
lds r20,B0_W_R1 ;Загрузка целой dX[n]части b0 в целую часть множимого
muls r16,r20 ;Перемножение целой части b0 и dX[n] со знаком
add r19,r0 ;Добавление младшего байта результата к Y[n]
lds r17,Del1DataR1 ;Загрузка dX[n-1] в множитель
lds r20,B1_F_R1 ;Загрузка дробной части b1 в дробную часть множимого
mul r17,r20 ;Перемножение дробной части b1 и dX[n-1] без знака
tst r17 ;Проверка знака dX[n-1]
brmi PiLoop13 ;Переход если отрицательный
sub r18,r0 ;Вычитание младшего байта результата из Y[n]
sbc r19,r1 ;Вычитание старшего байта результата из Y[n]
rjmp PiLoop14 ;Переход дальше
PiLoop13:
sub r18,r0 ;Вычитание младшего байта результата из Y[n]
brcc PiLoop15 ;Переход если не было переноса
inc r19 ;Добавление переноса к уменьшаемому
PiLoop15:
sub r19,r1 ;Вычитание старшего байта результата из Y[n]
PiLoop14:
lds r20,B1_W_R1 ;Загрузка целой части b1 в дробную часть множимого
muls r17,r20 ;Перемножение целой части b1 и dX[n-1] без знака
sub r19,r0 ;Вычитание младшего байта результата из Y[n]
sts Del1DataR1,r16 ;Сохранение dX[n] в dX[n-1]
lds r20,Out1DataR1l ;Считывание дробной части значения Y[n-1]
lds r16,Out1DataR1h ;Считывание целой части значения Y[n-1]
add r18,r20
adc r16,r19 ;Добавление Y[n-1]
;Сравнение Y[n] с минимальным значением
lds r17,MinOutDataR1 ;Загрузка минимального значения рассогласования регулятора (Ymin)
tst r17 ;Проверка знака Ymin
brmi PiLoop07 ;Переход если отрицательное
tst r16 ;Проверка знака Y[n]
brmi PiLoop08 ;Переход если отрицательное (Y[n] меньше Ymin)
cp r16,r17 ;Сравнение Y[n] и Ymin
brpl PiLoop09 ;Переход если Y[n] больше Ymin
PiLoop08:
mov r16,r17 ;Загрузка Ymin в Y[n]
clr r18 ;Обнуление дробной части Y[n]
rjmp PiLoop09 ;Переход дальше
PiLoop07:
tst r16 ;Проверка знака Y[n]
brpl PiLoop09 ;Переход если положительное (Y[n] больше Ymin)
cp r16,r17 ;Сравнение Y[n] и Ymin
brmi PiLoop08 ;Переход если Y[n] меньше Ymin
;Сравнение Y[n] с максимальным значением
PiLoop09:
lds r17,MaxDelDataR1 ;Загрузка максимального значения рассогласования регулятора (Ymax)
tst r17 ;Проверка знака Ymax
brmi PiLoop10 ;Переход если отрицательное
tst r16 ;Проверка знака Y[n]
brmi PiLoop12 ;Переход если отрицательное (Y[n] меньше Ymax)
cp r17,r16 ;Сравнение Ymax и Y[n]
brpl PiLoop12 ;Переход если Y[n] меньше Ymax
PiLoop11:
mov r16,r17 ;Загрузка Ymax в Y[n]
clr r18 ;Обнуление дробной части Y[n]
rjmp PiLoop12 ;Переход дальше
PiLoop10:
tst r16 ;Проверка знака Y[n]
brpl PiLoop11 ;Переход если положительное (Y[n] больше Ymax)
cp r16,r17 ;Сравнение Y[n] и Ymin
brmi PiLoop11 ;Переход если Y[n] меньше Ymin
PiLoop12:
sts Out1DataR1l,R18 ;Сохранение дробной части Y[n] в Y[n-1]
sts Out1DataR1h,R16 ;Сохранение целой части Y[n] в Y[n-1]
pop r20
pop r19
pop r18
pop r17
pop r16
pop r1
pop r0 ;Восстановление регистров из стека
ret ;Возврат из подпрограммы
;*** Подпрограмма выбора скважности из таблицы
;Вход: Ang_Reg - текущий угол фазы
;Выход: PWM_Reg - скважность, Key_Reg - разряды выбора ключей
CasePWM:
push r0
push r1
push Zl
push Zh
push Ang_Reg
push Tmp_Reg ;Сохранение регистров в стеке
cpi Ang_Reg,Ang90 ;Сравнение текущего угла с 90
brmi CasLp01 ;Переход если меньше
cpi Ang_Reg,Ang180 ;Сравнение текущего угла с 180
brmi CasLp04 ;Переход если меньше
cpi Ang_Reg,Ang270 ;Сравнение текущего угла с 270
brmi CasLp07 ;Переход если меньше
cpi Ang_Reg,Ang330 ;Сравнение текущего угла с 330
brpl CasLp11 ;Переход если болльше
ldi Key_Reg,0b10 ;Загрузка кода выбора отрицательного ключа
rjmp CasLp12
CasLp11:
ldi Key_Reg,0b00 ;Загрузка кода отсутствия выбора ключа
CasLp12:
mov Tmp_Reg,Ang_Reg
ldi Ang_Reg,Ang360
sub Ang_Reg,Tmp_Reg ;Корректировка угла
rjmp CasLp03
CasLp01:
cpi Ang_Reg,Ang30 ;Сравнение текущего угла с 30
brmi CasLp02 ;Переход если меньше
ldi Key_Reg,0b01 ;Загрузка кода выбора положительного ключа
rjmp CasLp03
CasLp02:
ldi Key_Reg,0b00 ;Загрузка кода отсутствия выбора ключа
rjmp CasLp03
CasLp04:
cpi Ang_Reg,Ang150 ;Сравнение текущего угла с 150
brpl CasLp05 ;Переход если больше
ldi Key_Reg,0b01 ;Загрузка кода выбора положительного ключа
rjmp CasLp06
CasLp05:
ldi Key_Reg,0b00 ;Загрузка кода отсутствия выбора ключа
CasLp06:
mov Tmp_Reg,Ang_Reg
ldi Ang_Reg,Ang180
sub Ang_Reg,Tmp_Reg ;Корректировка угла
rjmp CasLp03
CasLp07:
cpi Ang_Reg,Ang210 ;Сравнение текущего угла с 210
brmi CasLp08 ;Переход если меньше
ldi Key_Reg,0b10 ;Загрузка кода выбора отрицательного ключа
rjmp CasLp09
CasLp08:
ldi Key_Reg,0b00 ;Загрузка кода отсутствия выбора ключа
CasLp09:
ldi Tmp_Reg,Ang180
sub Ang_Reg,Tmp_Reg ;Корректировка угла
CasLp03:
ldi Zl,TabStartl
ldi Zh,TabStarth
clr Tmp_Reg
add Zl,Ang_Reg
adc Zh,Tmp_Reg ;Формирование адреса строки таблицы
out EEARl,Zl
out EEARh,Zh ;Загрузка адреса строки таблицы в регистр адреса EEPROM
sbi EECR,EERE ;Команда считывания данных из EEPROM
in Tmp_Reg,EEDR ;Считывание скважности из регистра данных EEPROM
lds r0,Ampltd ;Загрузка значения амплитуды
mul r0,Tmp_Reg ;Формирование требуемой скважности
mov PWM_Reg,r1 ;Загрузка значения скважности (целой части из старшего байта произведения)
pop Tmp_Reg
pop Ang_Reg
pop Zh
pop Zl
pop r1
pop r0 ;Восстановление регистров из стека
ret ;Возврат из подпрограммы