ПИД-регулятор своими руками

; IntMoment := 12V
; Max := sqr(220*sqrt(2))
; { Sqr( Sin(Pi/2)*K ) = Max
; { Sqr( Sin(X)*K ) = IntMoment
;
; 2*k/MaxCode = 1 - cos(T*Pi)
;   cos(T*Pi) = 1-2*k/MaxCode
;   T*Pi = arccos(1-2*k/MaxCode)
;   T = arccos(1-2*k/MaxCode) / Pi

#!/usr/bin/perl
# (T*Pi)/2 - sin(2*T*Pi)/4 = Q*Pi/2
use constant PI    => 4 * atan2(1, 1);

$T = 1;
for( $i = 250; $i >= 0; $i-- ) {
        $int = $i*PI/2/250;
        $ev = ($T*PI)/2-sin(2*$T*PI)/4;
        while( abs($ev-$int) > 0.0005 ) {
                $T -= 0.0001;
                $ev = ($T*PI)/2-sin(2*$T*PI)/4;
        }
        #print $i."\t".$T."\n";
        $code = 65536-($T*(32768*384/2)/100.0 + 773);
        printf "DB 0%02Xh, 0%02Xh ; %04Xh = $i/250 of power\n", $code%256, int($code/256), $code, $i;
}

; PID управление
CalcMainEnd:
	; Вычисления, Go-Go.
CalcPid:
	;	1. Eo = E				 | 16bit
Pid1:
	MOV Err0H, ErrH
	MOV Err0L, ErrL
	;	2. E = Y-X				 | 16bit
Pid2:
	CLR C
	MOV A, SettingL
	SUBB A, ThermoL
	MOV ErrL, A
	MOV A, SettingH
	SUBB A, ThermoH
	MOV ErrH, A
	JNB  OV, Pid2Ov
	JB	ACC.7, Pid2Max
Pid2Min:
	MOV ErrL, #LOW(-500*32)
	MOV ErrH, #HIGH(-500*32)
	SJMP Pid2End
Pid2Max:
	MOV ErrL, #LOW(500*32)
	MOV ErrH, #HIGH(500*32)
	SJMP Pid2End
Pid2Ov:
	JNB ACC.7, Pid2OvP
Pid2OvN: ; Проверим на ограничение вниз
	CLR C
	MOV A, ErrL
	SUBB A, #LOW(-500*32)
	MOV A, ErrH
	SUBB A, #HIGH(-500*32)
	JNC Pid2End ; Если > -500 => всё ок
	SJMP Pid2Min
Pid2OvP:
	CLR C
	MOV A, ErrL
	SUBB A, #LOW(500*32)
	MOV A, ErrH
	SUBB A, #HIGH(500*32)
	JNC Pid2Max ; Если < 500 => всё ок
Pid2End:

	;	3. Int = Int + (E+Eo)/2  | 32bit+16bit
Pid3:
	JNB PowerReady, Pid3End ; Если нет сети -- интегральную часть не копим
	MOV A, ErrL
	ADD A, Err0L
	MOV R0, A ; временно
	MOV A, ErrH
	ADDC A, Err0H
	MOV C, ACC.7 ; Полусумма всегда влезает в 16 бит, поэтому при сдвиге надо сохранить знак
	RRC A	; Поделим без потери знака
	XCH A, R0 ; A= младшая часть, R0 - старшая часть полусуммы
	RRC A	; Доделили
	JNB  IntS, Pid3IntPos
	; Int отрицательный, изменим знак для R0:A, тем самым можно будет просто сложить с Int
	CLR C
	CPL A
	ADD A, #1
	XCH A, R0
	CPL A
	ADDC A, #0
	XCH A, R0
Pid3IntPos:
	; У Int и R0:A сейчас согласованы знаки, поэтому складываем обычным образом
	ADD A, IntLL
	MOV IntLL, A
	MOV A, IntLH
	ADDC A, R0
	MOV IntLH, A
	MOV A, R0
	JB	ACC.7, Pid3Neg ; Прибавляли отрицательную разность?
	; Если разность положительная, просто распространим перенос
	JNC jPid3End  ; Если прибавили слово и переноса небыло -- делать нам ничего не требуется.
	INC IntHL ; Распространяем перенос выше
	MOV A, IntHL
	JNZ Pid3End  ; Если перенос не ушел в 4й байт -- всё нормально
	INC IntHH ; Распространяем перенос на САМЫЙ старший байт
	MOV A, IntHH
	JNZ Pid3End  ; Если перенос не ушел еще выше -- всё нормально
	MOV IntHH, #0FFh ; Если перенс был выше -- ограничиваем интеграл потолком
	MOV IntHL, #0FFh
	MOV IntLH, #0FFh
	MOV IntLL, #0FFh
jPid3End:
	SJMP Pid3End
Pid3Neg: ; Если разность отрицательная, то надо продолжать добавлять оба раза, но FFh
	MOV A, IntHL
	ADDC A, #0FFh
	MOV IntHL, A
	MOV A, IntHH
	ADDC A, #0FFh
	MOV IntHH, A
	JC	Pid3End  ; Если тут был перенос, значит знак интеграла не изменился
	CPL IntS ; Если переноса небыло, значит у интеграла изменился знак
	CPL C		 ; Обратим знак получившегося числа
	MOV A, #0
	SUBB A, IntLL
	MOV IntLL, A
	MOV A, #0
	SUBB A, IntLH
	MOV IntLH, A
	MOV A, #0
	SUBB A, IntHL
	MOV IntHL, A
	MOV A, #0
	SUBB A, IntHH
	MOV IntHH, A
	; так как оно стало отрицательным -- то перенос тут будет всегда
Pid3End:

	;	5. cI = Int*(1/Ti)		 | 32*32=>32bit
Pid5: ; R3:R2:R1:R0 = Int*(1/Ti)
	JB Ti_sh1, Pid5Calc ; если Ti_sh1=0, то 1/Ti=1 или Ti=0. и ничего делать не надо
	MOV A, Ti_mLL
	ORL A, Ti_mLH
	ORL A, Ti_mHL
	ORL A, Ti_mHH
	JZ	Pid5Zero
	MOV R0, IntLL
	MOV R1, IntLH
	MOV R2, IntHL
	MOV R3, IntHH
	AJMP Pid5End
Pid5Zero:
	MOV A, #0
	MOV R0, A
	MOV R1, A
	MOV R2, A
	MOV R3, A
	MOV IntLL, A
	MOV IntLH, A
	MOV IntHL, A
	MOV IntHH, A
	AJMP Pid5End
Pid5Calc:
	; R7:R6:R5:R4[:R3] = MULUH(Int*Ti_m) // R3 считаем как часть для округления
	MOV R2, #0
	;; R7:R6 = IntHH*Ti_mHH
	MOV A, IntHH
	MOV B, Ti_mHH
	MUL AB
	MOV R7, B
	MOV R6, A
	; R6:R5 += IntHL*Ti_mHH
	MOV A, IntHL
	MOV B, Ti_mHH
	MUL AB
	MOV R5, A
	MOV A, R6
	ADD A, B
	MOV R6, A
	MOV A, R2 ; A=0
	ADDC A, R7
	MOV R7, A
	; R5:R4 += IntLH*Ti_mHH
	MOV A, IntLH
	MOV B, Ti_mHH
	MUL AB
	MOV R4, A
	MOV A, R5
	ADD A, B
	MOV R5, A
	MOV A, R2 ; A=0
	ADDC A, R6
	MOV R6, A
	MOV A, R2 ; A=0
	ADDC A, R7
	MOV R7, A
	; R4:R3 += IntLL*Ti_mHH
	MOV A, IntLL
	MOV B, Ti_mHH
	MUL AB
	MOV R3, A
	MOV A, R4
	ADD A, B
	MOV R4, A
	MOV A, R2 ; A=0
	ADDC A, R5
	MOV R5, A
	MOV A, R2 ; A=0
	ADDC A, R6
	MOV R6, A
	MOV A, R2 ; A=0
	ADDC A, R7
	MOV R7, A
	;; R6:R5 += IntHH*Ti_mHL
	MOV A, IntHH
	MOV B, Ti_mHL
	MUL AB
	ADD A, R5
	MOV R5, A
	MOV A, R6
	ADDC A, B
	MOV R6, A
	MOV A, R2 ; A=0
	ADDC A, R7
	MOV R7, A
	; R5:R4 += IntHL*Ti_mHL
	MOV A, IntHL
	MOV B, Ti_mHL
	MUL AB
	ADD A, R4
	MOV R4, A
	MOV A, R5
	ADDC A, B
	MOV R5, A
	MOV A, R2 ; A=0
	ADDC A, R6
	MOV R6, A
	MOV A, R2 ; A=0
	ADDC A, R7
	MOV R7, A
	; R4:R3 += IntLH*Ti_mHL
	MOV A, IntLH
	MOV B, Ti_mHL
	MUL AB
	MOV A, R3
	MOV R3, A
	MOV A, R4
	ADDC A, B
	MOV R4, A
	MOV A, R2 ; A=0
	ADDC A, R5
	MOV R5, A
	MOV A, R2 ; A=0
	ADDC A, R6
	MOV R6, A
	MOV A, R2 ; A=0
	ADDC A, R7
	MOV R7, A
	;; R5:R4 += IntHH*Ti_mLH
	MOV A, IntHH
	MOV B, Ti_mLH
	MUL AB
	ADD A, R4
	MOV R4, A
	MOV A, R5
	ADDC A, B
	MOV R5, A
	MOV A, R2 ; A=0
	ADDC A, R6
	MOV R6, A
	MOV A, R2 ; A=0
	ADDC A, R7
	MOV R7, A
	; R4:R3 += IntHL*Ti_mLH
	MOV A, IntHL
	MOV B, Ti_mLH
	MUL AB
	ADD A, R3
	MOV R3, A
	MOV A, R4
	ADDC A, B
	MOV R4, A
	MOV A, R2 ; A=0
	ADDC A, R5
	MOV R5, A
	MOV A, R2 ; A=0
	ADDC A, R6
	MOV R6, A
	MOV A, R2 ; A=0
	ADDC A, R7
	MOV R7, A
	;; R4:R3 += IntHH*Ti_mLL
	MOV A, IntHH
	MOV B, Ti_mLL
	MUL AB
	ADD A, R3
	MOV R3, A
	MOV A, R4
	ADDC A, B
	MOV R4, A
	MOV A, R2 ; A=0
	ADDC A, R5
	MOV R5, A
	MOV A, R2 ; A=0
	ADDC A, R6
	MOV R6, A
	MOV A, R2 ; A=0
	ADDC A, R7
	MOV R7, A
	;;; Если R3 > 7F --
	MOV A, R3
	JNB ACC.7, Pid5Shift ; Если R3<80 -- округление не надо
	ANL A, #7Fh
	JZ	Pid5Round ; Если = 80 -- округляем до нечетного
	MOV A, #1
	ADD A, R4
	MOV R4, A
	MOV A, R2 ; A=0
	ADDC A, R5
	MOV R5, A
	MOV A, R2 ; A=0
	ADDC A, R6
	MOV R6, A
	MOV A, R2 ; A=0
	ADDC A, R7
	MOV R7, A
	SJMP Pid5Shift
Pid5Round:
	MOV A, R4
	ORL A, #01h
	MOV R4, A
	;JMP Pid5Shift

Pid5Shift:
	; R3:R2:R1:R0 = (Int-R7:R6:R5:R4) >> 1
	CLR C
	MOV A, IntLL
	SUBB A, R4
	MOV R0, A
	MOV A, IntLH
	SUBB A, R5
	MOV R1, A
	MOV A, IntHL
	SUBB A, R6
	MOV R2, A
	MOV A, IntHH
	SUBB A, R7
	RRC A ; >>1 без потери переноса
	MOV R3, A
	MOV A, R2
	RRC A
	MOV R2, A
	MOV A, R1
	RRC A
	MOV R1, A
	MOV A, R0
	RRC A
	;MOV R0, A
	; R3:R2:R1:R0 += R7:R6:R5:R4
	;MOV A, R0
	ADD A, R4
	MOV R0, A
	MOV A, R1
	ADDC A, R5
	MOV R1, A
	MOV A, R2
	ADDC A, R6
	MOV R2, A
	MOV A, R3
	ADDC A, R7
	MOV R7, A
	; Теперь сдвинуть вправо на sh2.
	; sh2 может быть до 16 (так как у нас Ti 16разрядный; проверим необходимость сдвига на 16 бит)
	MOV A, Ti_sh2
	JNB ACC.4, Pid5ShiftUnder16
	; Надо сдвинуть >=16 -- 2 байта сдвинем mov'ами
	MOV R0, 18h+2; R2, bank 3
	MOV R1, 18h+3; R3, bank 3
	MOV R2, #0
	MOV R3, #0
Pid5ShiftUnder16:
	JNB ACC.3, Pid5ShiftUnder8
	; Надо сдвинуть на >=8 -- 1 байт сдвигаем mov'ами
	MOV R0, 18h+1; R1, bank 3
	MOV R1, 18h+2; R2, bank 3
	MOV R2, 18h+3; R3, bank 3
	MOV R3, #0
Pid5ShiftUnder8:
	ANL A, #07h
	JZ Pid5End ; Если внутри байта двигать не надо -- всё
	MOV R4, A
	SJMP Pid5ShiftRight
Pid5NextShift:
	CLR C
	; К этому моменту C у нас еще возможнозначимый старший бит!
Pid5ShiftRight:
	MOV A, R3
	RRC A
	MOV R3, A
	MOV A, R2
	RRC A
	MOV R2, A
	MOV A, R1
	RRC A
	MOV R1, A
	MOV A, R0
	RRC A
	MOV R0, A
	DJNZ R4, Pid5NextShift
	; Всё, после всех сдвигов получили результат
	; Не забываем, что у вычисленного в R3:R2:R1:R0
	; сейчас число положительное, а знак его в IntS
Pid5End:

	;	4. PID += [ cD = Td * (E-Eo) ]		| 16*16=>32bit
Pid4: ; cD = R7:R6:R5:R4; ErrD = E-Eo
	CLR C
	MOV A, ErrL
	SUBB A, Err0L
	MOV DiffL, A
	MOV A, ErrH
	SUBB A, Err0H
	MOV DiffH, A
	MOV C, ACC.7 ; Берём знак результата
	MOV DiffS, C ; Сохраним знак E-Eo
	JNC Pid4Mul
	; Diff -- орицательный, обратим знак
	MOV A, DiffL
	CPL A
	ADD A, #1
	MOV DiffL, A
	MOV A, DiffH
	CPL A
	ADDC A, #0
	MOV DiffH, A
Pid4Mul:
	; R7:R6 = DiffH*TdH
	; MOV A, DiffH = в любом случае A=DiffH
	MOV B, TdH
	MUL AB
	MOV R6, A
	MOV R7, B
	; R5:R4 = DiffL*TdL
	MOV A, DiffL
	MOV B, TdL
	MUL AB
	MOV R4, A
	MOV R5, B
	; R6:R5 += DiffH*TdL
	MOV A, DiffH
	MOV B, TdL
	MUL AB
	ADD A, R5
	MOV R5, A
	MOV A, R6
	ADD A, B
	MOV R6, A
	MOV A, R7
	ADDC A, #0
	MOV R7, A
	; R6:R5 += DiffL*TdH
	MOV A, DiffL
	MOV B, TdH
	MUL AB
	ADD A, R5
	MOV R5, A
	MOV A, R6
	ADD A, B
	MOV R6, A
	MOV A, R7
	ADDC A, #0
	MOV R7, A

	;	6. PID = E + cI + cD	 | 32bit
Pid6:	; R3:R2:R1:R0 равно cI, знак в IntS;
	; R7:R6:R5:R4 = cD; знак в DiffS
	; E в обратном дополнительном коде

	JB IntS, ChkDiffN
	JNB DiffS, Pid6Add ; Int>0, Diff>0 => Add
	SJMP Pid6Sub ; Int>0, Diff<0 => Sub
ChkDiffN:
	JNB DiffS, Pid6Sub ; Int<0, Diff>0 => Sub
	; Int<0, Diff<0 => Add
Pid6Add:
	; Одинаковый знак => складываем их с проверкой на переполнение
	MOV A, R0
	ADD A, R4
	MOV R0, A
	MOV A, R1
	ADDC A, R5
	MOV R1, A
	MOV A, R2
	ADDC A, R6
	MOV R2, A
	MOV A, R3
	ADDC A, R7
	MOV R3, A
	JNC Pid6Err ; Если нет переноса - в результате сложения переполнения небыло
	MOV R3, #0FFh
	MOV R2, #0FFh
	MOV R1, #0FFh
	MOV R0, #0FFh
	SJMP Pid6Err
Pid6Sub:
	; Знаки разные -- вычтем одно из другого и проверим знак результата
	CLR C
	MOV A, R4
	SUBB A, R0
	MOV R0, A
	MOV A, R5
	SUBB A, R1
	MOV R1, A
	MOV A, R6
	SUBB A, R2
	MOV R2, A
	MOV A, R7
	SUBB A, R3
	MOV R3, A
	JNC Pid6Err ; Если нет заимствования -- знак результата равен знаку DiffS
	CPL DiffS ; Если заимствование было, у DiffS и результата надо обратить знак
	MOV R6, #0 ; R6=0
	MOV A, R0
	CPL A
	ADDC A, R6 ; R6=0, C=1 => действие +1
	MOV R0, A
	MOV A, R1
	CPL A
	ADDC A, R6 ; +перенос
	MOV R1, A
	MOV A, R2
	CPL A
	ADDC A, R6
	MOV R2, A
	MOV A, R3
	CPL A
	ADDC A, R6
	MOV R3, A

Pid6Err:
	MOV R6, #0 ; R6=0
	; В R3:R2:R1:R0 -- лежит cI+cD; знак суммы в DiffS
	; надо прибавить/отнять Err, записанное в обратном коде
	; Приведём знак Err к DiffS
	MOV R4, ErrL
	MOV A, ErrH
	JB ACC.7, Pid6ChkDiffS
	JNB DiffS, Pid6SumErrNoInv ; Err>0, Diff>0 => NoInv
	SJMP Pid6SumErrInv
Pid6ChkDiffS:
	JNB DiffS, Pid6SumErrNoInv ; Err<0, Diff>0 => NoInv
Pid6SumErrInv:
	; У Err знак отличается от DiffS -- инвертируем
	SETB C ; Не уверен в состоянии C
	MOV A, ErrL
	CPL A
	ADDC A, R6 ; A+=R6+C, R6=0	C=1 => A+=1
	MOV R4, A ; R4=ErrL
	MOV A, ErrH
	CPL A
	ADDC A, R6
Pid6SumErrNoInv:
	MOV R5, A ; ErrH
Pid6SumErr:
	; Итак, в R5:R4 лежит Err, знак которого согласован с DiffS; но в обратно-дополнительном коде
	MOV A, R0
	ADD A, R4
	MOV R0, A
	MOV A, R5
	CLR F0
	JNB ACC.7, Pid6SubErrPos
	SETB F0
	MOV R6, #0FFh ; Добавляем отрицательное => дополняем FFами
Pid6SubErrPos:
	ADDC A, R1
	MOV R1, A
	MOV A, R2
	ADDC A, R6 ; +расширение
	MOV R2, A
	MOV A, R3
	ADDC A, R6 ; +расширение
	MOV R3, A
	MOV R6, #0
	; Надо проверить нет ли смены знака итоговой суммы
	JNC Pid6ChkF0
	JB F0, Pid7 ; Err<0, был перенос => Знак не сменился, переполнения нет
	SJMP Pid6SumOv ; Err>0, был перенос => переполнение
Pid6ChkF0:
	JNB F0, Pid7 ; Err>0, небыло переноса => нет переполнения
	;SJMP Pid6SumUf ; Err<0, небыло переноса => сменился знак
Pid6SumUf:
	; Если Err<0 и небыло переноса => сменился знак
	CPL DiffS
	MOV A, R0
	CPL A
	ADD A, #1 ; C=?, поэтому прибавляем 1 обычным методом
	MOV R0, A
	MOV A, R1
	CPL A
	ADDC A, R6
	MOV R1, A
	MOV A, R2
	CPL A
	ADDC A, R6
	MOV R2, A
	MOV A, R3
	CPL A
	ADDC A, R6
	MOV R3, A
	SJMP Pid7 ; Знак у результата и DiffS приведены в норму
Pid6SumOv:
	; Было переполнение => округляем до максимума
	MOV R0, #0FFh
	MOV R1, #0FFh
	MOV R2, #0FFh
	MOV R3, #0FFh

	;	7. U = K*PID/256		 | 32bit*16bit/8bit => 40bit,
	;					 | которые усекаются до 10bit
	;					 | при вычислениях
Pid7: ; В R3:R2:R1:R0 лежит результат PID, в DiffS его знак
	  ; Нужно вычислить K*PID/256, ограничив результат до 10бит
	  ; K всегда положительно, поэтому если PID < 0 => минимум
	JB DiffS, Pid7Umin
	; поскольку мы можем жестко ограничить сверху 16ю битами,
	; то если R3 != 0 => ставим максимум в любом случае
	MOV A, R3
	JNZ Pid7Umax
	; [R2:R1:R0 * KH:HL] = [R7:R6:R5:R4:R3]
	; вычисляем, учитывая что должно получиться R7=0 R6=0,
	; иначе переполнение, поэтому R7 и R6 вообще не трогаем
	; но проверяем результат
	; R7:R6 = R2*KH
	MOV A, R2
	JZ Pid7S1
	MOV A, KH
	JNZ Pid7Umax ; Если R2!=0 и KH!=0 => R7:R6>0 => переполнение
Pid7S1:
	; R6:R5 = R2*KL
	MOV A, R2
	MOV B, KL
	MUL AB
	MOV R5, A
	MOV A, B
	JNZ Pid7Umax ; Если R6 > 0 => переполнение
	; R6:R5 = R1*KH
	MOV A, R1
	MOV B, KH
	MUL AB
	ADD A, R5
	JC	Pid7Umax ; Если R6 > 0 => переполнение
	MOV R5, A
	MOV A, B
	JNZ Pid7Umax ; Если R6 > 0 => переполнение
	; R5:R4 = R0*KH
	MOV A, R0
	MOV B, KH
	MUL AB
	MOV R4, A
	MOV A, R5
	ADD A, B
	JC	Pid7Umax ; Если R6 > 0 => переполнение
	MOV R5, A
	; R5:R4 = R1*KL
	MOV A, R1
	MOV B, KL
	MUL AB
	ADD A, R4
	MOV R4, A
	MOV A, R5
	ADDC A, B
	JC	Pid7Umax ; Если R6 > 0 => переполнение
	MOV R5, A
	; R4:R3 = R0*KL
	MOV A, R0
	MOV B, KL
	MUL AB
	RLC A ; C = R3>=0x80, Z=R3>0x80
	MOV R3, #0FFh ; R3<>0x80 => ничего
	JNZ Pid7S2
	MOV R3, #0FEh ; R3==0x80 => округление до четного
Pid7S2:
	MOV A, R4
	ADDC A, B ; Складываем умножение, регистр, и перенос-округление
	ANL A, R3 ; А так же если округление до четного -- отбрасываем после младший бит
	MOV R4, A
	MOV A, R5
	ADDC A, R6 ; R6=0 у нас с давних пор, хоть мы туда и не складывали ничего во время перемножения
	JC	Pid7Umax ; Если R6 > 0 => переполнение
	MOV R5, A
	; R5:R4 => ограниченный в 16 бит результат
	; Теперь надо ограничить R5:R4 до Umax/Umin
	MOV A, UmaxL
	SUBB A, R4 ; C=0 на текущий момент
	MOV A, UmaxH
	SUBB A, R5
	JC Pid7Umax ; Если R5:R4>Umax => R5:R4 = Umax
	MOV A, UminL
	SUBB A, R4 ; C=0 на текущий момент
	MOV A, UminH
	SUBB A, R5
	JNC Pid7Umin ; Если R5:R4<Umin => R5:R4 = Umin
	; Мощность вычислена
	MOV UH, R5
	MOV UL, R4
	SETB UReady
	AJMP CalcExit
Pid7Umax: ; Установить максимальную мощность
	MOV UH, UmaxH
	MOV UL, UmaxL
	SETB UReady
	AJMP CalcExit
Pid7Umin: ; Установить минимальную мощность
	MOV UH, UminH
	MOV UL, UminL
	SETB UReady
	AJMP CalcExit