1

2 ; Local inclusions.
3 .nolist
4 .include "dspcommon.inc" ; fractsetup
5 .list
6
7 .equ offsetabcCoefficients, 0
8 .equ offsetcontrolHistory, 2
9 .equ offsetcontrolOutput, 4
10 .equ offsetmeasuredOutput, 6
11 .equ offsetcontrolReference, 8
12 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
13 .section .libdsp, code
14 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
15 ;
16 ; _PID:
17 ; Prototype:
18 ; tPID PID ( tPID *fooPIDStruct )
19 ;
20 ; Operation:
21 ;
22 ; ---- Proportional
23 ; | | Output
24 ; ---------------| Kp |-----------------
25 ; | | | |
26 ; | ---- |
27 ;Reference | ---
28 ;Input --- | -------------- Integral | + | Control
-------
29 ; --------| + | Control | | Ki | Output | | Output
| |
30 ; | |----------|----------| ------------ |----------|+ |----------|
Plant |--
31 ; -----| - |Difference| | 1 - Z^(-1) | | |
| | |
32 ; | --- (error) | -------------- | + |
------- |
33 ; | |
--- |
34 ; | Measured | ------------------- Deriv
| |
35 ; | Outut | | | Output
| |
36 ; | --------| Kd * (1 - Z^(-1))
|--------- |
37 ; | |
| |
38 ; |
------------------- |
39 ;
| |
40 ;
| |
41 ;
-----------------------------------------------------------------------------------
42 ;
43 ; controlOutput[n] = controlOutput[n-1]
44 ; + controlHistory[n] * abcCoefficients[0]
45 ; + controlHistory[n-1] * abcCoefficients[1]
46 ; + controlHistory[n-2] * abcCoefficients[2]
47 ;
48 ; where:
49 ; abcCoefficients[0] = Kp + Ki + Kd
50 ; abcCoefficients[1] = -(Kp + 2*Kd)
51 ; abcCoefficients[2] = Kd
52 ; controlHistory[n] = measuredOutput[n] - referenceInput[n]
53 ; where:
54 ; abcCoefficients, controlHistory, controlOutput, measuredOutput and
controlReference
55 ; are all members of the data structure tPID.
56 ;
57 ; Input:
58 ; w0 = Address of tPID data structure
 59
60 ; Return:
61 ; w0 = Address of tPID data structure
62 ;
63 ; System resources usage:
64 ; {w0..w5} used, not restored
65 ; {w8,w10} saved, used, restored
66 ; AccA, AccB used, not restored
67 ; CORCON saved, used, restored
68 ;
69 ; DO and REPEAT instruction usage.
70 ; 0 level DO instruction
71 ; 0 REPEAT intructions
72 ;
73 ; Program words (24-bit instructions):
74 ; 28
75 ;
76 ; Cycles (including C-function call and return overheads):
77 ; 30
78 ;............................................................................
79
80 .global _PID ; provide global scope to routine
81 _PID:
82 ;btg LATD, #1
83 ; Save working registers.
84 push.s
85 push w4
86 push w5
87 push w8
88 push w10
89
90 push CORCON ; Prepare CORCON for fractional computation.
91
92 fractsetup w8
93
94 mov [w0 + #offsetabcCoefficients], w8 ; w8 = Base Address of
_abcCoefficients array [(Kp+Ki+Kd), -(Kp+2Kd), Kd]
95 mov [w0 + #offsetcontrolHistory], w10 ; w10 = Address of _ControlHistory
array (state/delay line)
96
97 mov [w0 + #offsetcontrolOutput], w1
98 mov [w0 + #offsetmeasuredOutput], w2
99 mov [w0 + #offsetcontrolReference], w3
100
101 ; Calculate most recent error with saturation, no limit checking required
102 lac w3, a ; A = tPID.controlReference
103 lac w2, b ; B = tPID.MeasuredOutput
104 sub a ; A = tPID.controlReference -
tPID.measuredOutput
105 sac.r a, [w10] ; tPID.ControlHistory[n] = Sat(Rnd(A))
106
107 ; Calculate PID Control Output
108 clr a, [w8]+=2, w4, [w10]+=2, w5 ; w4 = (Kp+Ki+Kd), w5 =
_ControlHistory[n]
109 lac w1, a ; A = ControlOutput[n-1]
110 mac w4*w5, a, [w8]+=2, w4, [w10]+=2, w5 ; A += (Kp+Ki+Kd) *
_ControlHistory[n]
111 ; w4 = -(Kp+2Kd), w5 =
_ControlHistory[n-1]
112 mac w4*w5, a, [w8], w4, [w10]-=2, w5 ; A += -(Kp+2Kd) *
_ControlHistory[n-1]
113 ; w4 = Kd, w5 =
_ControlHistory[n-2]
114 mac w4*w5, a, [w10]+=2, w5 ; A += Kd *
_ControlHistory[n-2]
115 ; w5 = _ControlHistory[n-1]
116 ; w10 = &_ControlHistory[n-2]
117 sac.r a, w1 ; ControlOutput[n] =
Sat(Rnd(A))
118
119 mov w1, [w0 + #offsetcontrolOutput]
120
121 ;Update the error history on the delay line
122 mov w5, [w10] ; _ControlHistory[n-2] = _ControlHistory[n-1]
123 mov [w10 + #-4], w5 ; _ControlHistory[n-1] = ControlHistory[n]
124 mov w5, [--w10]
125
126 pop CORCON ; restore CORCON.
127 pop w10 ; Restore working registers.
128 pop w8
129 pop w5
130 pop w4
131 pop.s
132 return
133 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
134 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
135 ; _PIDInit:
136 ;
137 ; Prototype:
138 ; void PIDInit ( tPID *fooPIDStruct )
139 ;
140 ; Operation: This routine clears the delay line elements in the array
141 ; _ControlHistory, as well as clears the current PID output
142 ; element, _ControlOutput
143 ;
144 ; Input:
145 ; w0 = Address of data structure tPID (type defined in dsp.h)
146 ; Return:
147 ; (void)
148 ;
149 ; System resources usage:
150 ; w0 used, restored
151 ;
152 ; DO and REPEAT instruction usage.
153 ; 0 level DO instruction
154 ; 0 REPEAT intructions
155 ;
156 ; Program words (24-bit instructions):
157 ; 11
158 ;
159 ; Cycles (including C-function call and return overheads):
160 ; 13
161 ;............................................................................
162 .global _PIDInit ; provide global scope to routine
163 _PIDInit:
164 push w0
165 add #offsetcontrolOutput, w0 ;clear controlOutput
166 clr [w0]
167 pop w0
168 push w0
169 ;Set up pointer to the base of
170 ;controlHistory variables within tPID
171 mov [w0 + #offsetcontrolHistory], w0
172 ; Clear controlHistory variables
173 ; within tPID
174 clr [w0++] ; ControlHistory[n]=0
175 clr [w0++] ; ControlHistory[n-1] = 0
176 clr [w0] ; ControlHistory[n-2] = 0
177 pop w0 ;Restore pointer to base of tPID
178 return
179 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
180 .end
181 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
182 ; OEF
183
184