Подпрограмма выбора скважности для заданного угла

Подпрограмма определяет, в каком из четырех квадрантов находится заданный угол, и в зависимости от этого корректирует угол (до значения, не превышающего 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 ;Возврат из подпрограммы