📄 regul.ls
字号:
1 ; C Compiler for ST7 (COSMIC Software)
2 ; Generator V4.4a - 17 Dec 2001
3 ; Optimizer V4.2g - 21 Dec 2001
74 ; 64 u16 Period_To_Frequency(void)
74 ; 65 {
75 .text: section .text,new
77 xref.b _Period_To_Frequency$L
78 0000 _Period_To_Frequency:
81 ; 73 Ratio_Min = RATIO_MAX; // init Ratio_Min with max ratio
83 0000 a60f ld a,#15
84 0002 b7fe ld _Period_To_Frequency$L-2,a
85 ; 74 MZ_Temp = 0;
87 0004 3ffc clr _Period_To_Frequency$L-4
88 0006 3ffd clr _Period_To_Frequency$L-3
89 ; 76 for (i=0;i<=STEP_Z_BUFFER_SIZE-1;i++) // check max ratio of buffer
91 0008 3fff clr _Period_To_Frequency$L-1
92 000a L15:
93 ; 78 if (Step_Z[i].Ratio < Ratio_Min) Ratio_Min = Step_Z[i].Ratio;
95 000a beff ld x,_Period_To_Frequency$L-1
96 000c 58 sll x
97 000d d60006 ld a,(_Step_Z,x)
98 0010 b1fe cp a,_Period_To_Frequency$L-2
99 0012 2402 jruge L75
102 0014 b7fe ld _Period_To_Frequency$L-2,a
103 0016 L75:
104 ; 76 for (i=0;i<=STEP_Z_BUFFER_SIZE-1;i++) // check max ratio of buffer
106 0016 3cff inc _Period_To_Frequency$L-1
109 0018 b6ff ld a,_Period_To_Frequency$L-1
110 001a a106 cp a,#6
111 001c 25ec jrult L15
112 ; 82 for (i=0;i<=STEP_Z_BUFFER_SIZE-1;i++) // Compute average period
114 001e 3fff clr _Period_To_Frequency$L-1
115 0020 L16:
116 ; 84 if (Step_Z[i].Ratio == Ratio_Min) MZ_Temp += (u8)(Step_Z[i].StepTime);
118 0020 beff ld x,_Period_To_Frequency$L-1
119 0022 58 sll x
120 0023 d60006 ld a,(_Step_Z,x)
121 0026 b1fe cp a,_Period_To_Frequency$L-2
122 0028 260d jrne L76
125 002a d60007 ld a,(_Step_Z+1,x)
126 002d bbfd add a,_Period_To_Frequency$L-3
127 002f b7fd ld _Period_To_Frequency$L-3,a
128 0031 2431 jrnc L17
129 0033 3cfc inc _Period_To_Frequency$L-4
131 0035 202d jra L17
132 0037 L76:
133 ; 85 else MZ_Temp += ((Step_Z[i].StepTime)<<((u8)(Step_Z[i].Ratio-Ratio_Min)));
135 0037 d60007 ld a,(_Step_Z+1,x)
136 003a 5f clr x
137 003b 88 push a
138 003c 90beff ld y,_Period_To_Frequency$L-1
139 003f 9058 sll y
140 0041 90d60006 ld a,(_Step_Z,y)
141 0045 b0fe sub a,_Period_To_Frequency$L-2
142 0047 9097 ld y,a
143 0049 905d tnz y
144 004b 84 pop a
145 004c 2706 jreq L01
146 004e L21:
147 004e 48 sll a
148 004f 59 rlc x
149 0050 905a dec y
150 0052 26fa jrne L21
151 0054 L01:
152 0054 b7f7 ld _Period_To_Frequency$L-9,a
153 0056 bff6 ld _Period_To_Frequency$L-10,x
154 0058 b6fd ld a,_Period_To_Frequency$L-3
155 005a bbf7 add a,_Period_To_Frequency$L-9
156 005c b7fd ld _Period_To_Frequency$L-3,a
157 005e b6fc ld a,_Period_To_Frequency$L-4
158 0060 b9f6 adc a,_Period_To_Frequency$L-10
159 0062 b7fc ld _Period_To_Frequency$L-4,a
160 0064 L17:
161 ; 82 for (i=0;i<=STEP_Z_BUFFER_SIZE-1;i++) // Compute average period
163 0064 3cff inc _Period_To_Frequency$L-1
166 0066 b6ff ld a,_Period_To_Frequency$L-1
167 0068 a106 cp a,#6
168 006a 25b4 jrult L16
169 ; 89 result = (10*MTIM_MAX_FREQ)/MZ_Temp;
171 006c b6fd ld a,_Period_To_Frequency$L-3
172 006e befc ld x,_Period_To_Frequency$L-4
173 0070 cd0000 call c_uitol
175 0073 aef4 ld x,#_Period_To_Frequency$L-12
176 0075 cd0000 call c_rtol
178 0078 3f03 clr c_lreg+3
179 007a a62d ld a,#45
180 007c b702 ld c_lreg+2,a
181 007e a631 ld a,#49
182 0080 b701 ld c_lreg+1,a
183 0082 a601 ld a,#1
184 0084 b700 ld c_lreg,a
185 0086 cd0000 call c_ludv
187 0089 aef8 ld x,#_Period_To_Frequency$L-8
188 008b cd0000 call c_rtol
190 ; 90 result >>= (u8)(Ratio_Min); //divide by 2^ratio
192 008e b6fe ld a,_Period_To_Frequency$L-2
193 0090 cd0000 call c_lgursh
195 ; 92 return((u16)(result));
197 0093 b6fb ld a,_Period_To_Frequency$L-5
198 0095 befa ld x,_Period_To_Frequency$L-6
201 0097 81 ret
224 .const: section .text
225 0000 L61:
226 0000 00000a00 dc.l 2560
227 ; 96 void Init_PI(void)
227 ; 97 {
228 .text: section .text,new
229 0000 _Init_PI:
232 ; 102 VoltageIntegralTerm = (((ramp_MCPVH<<8) + ramp_MCPVL)>>3)*65536;
234 0000 4f clr a
235 0001 c70005 ld L3_VoltageIntegralTerm+3,a
236 0004 c70004 ld L3_VoltageIntegralTerm+2,a
237 0007 a64c ld a,#76
238 0009 c70003 ld L3_VoltageIntegralTerm+1,a
239 000c a605 ld a,#5
240 000e c70002 ld L3_VoltageIntegralTerm,a
241 ; 103 VoltageIntegralTerm /= PWM_FREQUENCY;
243 0011 ae02 ld x,#high(L3_VoltageIntegralTerm)
244 0013 bf00 ld c_x,x
245 0015 ae02 ld x,#low(L3_VoltageIntegralTerm)
246 0017 cd0000 call c_xltor
248 001a ae00 ld x,#high(L61)
249 001c bf00 ld c_x,x
250 001e ae00 ld x,#low(L61)
251 0020 cd0000 call c_xldiv
253 0023 ae02 ld x,#high(L3_VoltageIntegralTerm)
254 0025 bf00 ld c_x,x
255 0027 ae02 ld x,#low(L3_VoltageIntegralTerm)
257 ; 105 }
260 0029 cc0000 jp c_rtoxl
284 switch .const
285 0004 L22:
286 0004 00000006 dc.l 6
287 ; 107 void Adjust_INT_TERM_current(void)
287 ; 108 {
288 .text: section .text,new
289 0000 _Adjust_INT_TERM_current:
292 ; 109 VoltageIntegralTerm = VoltageIntegralTerm/6; // /4
294 0000 ae02 ld x,#high(L3_VoltageIntegralTerm)
295 0002 bf00 ld c_x,x
296 0004 ae02 ld x,#low(L3_VoltageIntegralTerm)
297 0006 cd0000 call c_xltor
299 0009 ae04 ld x,#high(L22)
300 000b bf00 ld c_x,x
301 000d ae04 ld x,#low(L22)
302 000f cd0000 call c_xldiv
304 0012 ae02 ld x,#high(L3_VoltageIntegralTerm)
305 0014 bf00 ld c_x,x
306 0016 ae02 ld x,#low(L3_VoltageIntegralTerm)
308 ; 110 }
311 0018 cc0000 jp c_rtoxl
335 ; 112 void Adjust_INT_TERM_voltage(void)
335 ; 113 {
336 .text: section .text,new
337 0000 _Adjust_INT_TERM_voltage:
340 ; 114 VoltageIntegralTerm = VoltageIntegralTerm*4; // *3
342 0000 ae02 ld x,#high(L3_VoltageIntegralTerm)
343 0002 bf00 ld c_x,x
344 0004 ae02 ld x,#low(L3_VoltageIntegralTerm)
345 0006 a602 ld a,#2
347 ; 115 }
350 0008 cc0000 jp c_xlglsh
455 switch .const
456 0008 L03:
457 0008 00000100 dc.l 256
458 000c L23:
459 000c 80000000 dc.l -2147483648
460 ; 126 u16 regul_PI(u16 Target_Freq) // return 10 bits value
460 ; 127 {
461 .text: section .text,new
463 xref.b _regul_PI$L
464 0000 _regul_PI:
466 0000 b701 ld _regul_PI$L+1,a
467 0002 bf00 ld _regul_PI$L,x
469 ; 135 Freq_Motor = (u16)Period_To_Frequency();
471 0004 cd0000 call _Period_To_Frequency
473 0007 c70001 ld _Freq_Motor+1,a
474 000a cf0000 ld _Freq_Motor,x
475 ; 137 Error = (s16)(Target_Freq - Freq_Motor); // Freq_Motor is actually the step time between 6 Z events
477 000d b601 ld a,_regul_PI$L+1
478 000f c00001 sub a,_Freq_Motor+1
479 0012 88 push a
480 0013 b600 ld a,_regul_PI$L
481 0015 c20000 sbc a,_Freq_Motor
482 0018 97 ld x,a
483 0019 84 pop a
484 001a b7ef ld _regul_PI$L-17,a
485 001c bfee ld _regul_PI$L-18,x
486 ; 138 if (Error > (s16)(Error_slip_MAX))
488 001e a001 sub a,#1
489 0020 9f ld a,x
490 0021 a208 sbc a,#8
491 0023 2b08 jrmi L571
492 ; 140 Error_slip = Error_slip_MAX;
494 0025 a608 ld a,#8
495 0027 b7fa ld _regul_PI$L-6,a
496 0029 3ffb clr _regul_PI$L-5
498 002b 2014 jra L771
499 002d L571:
500 ; 142 else if (Error < (s16)(Error_slip_MIN))
502 002d b6ee ld a,_regul_PI$L-18
503 002f a1f8 cp a,#248
504 0031 2a08 jrpl L102
505 ; 144 Error_slip = Error_slip_MIN;
507 0033 a6f8 ld a,#248
508 0035 b7fa ld _regul_PI$L-6,a
509 0037 3ffb clr _regul_PI$L-5
511 0039 2006 jra L771
512 003b L102:
513 ; 146 else Error_slip = (s16)Error;
515 003b bffa ld _regul_PI$L-6,x
516 003d b6ef ld a,_regul_PI$L-17
517 003f b7fb ld _regul_PI$L-5,a
518 0041 L771:
519 ; 152 Voltage_slip_s32 = Kp * (s32)Error_slip;
521 0041 c60000 ld a,_Kp
522 0044 b703 ld c_lreg+3,a
523 0046 4f clr a
524 0047 b702 ld c_lreg+2,a
525 0049 b701 ld c_lreg+1,a
526 004b b700 ld c_lreg,a
527 004d aee8 ld x,#_regul_PI$L-24
528 004f cd0000 call c_rtol
530 0052 b6fb ld a,_regul_PI$L-5
531 0054 befa ld x,_regul_PI$L-6
532 0056 cd0000 call c_itol
534 0059 aee8 ld x,#_regul_PI$L-24
535 005b cd0000 call c_lmul
537 005e aef6 ld x,#_regul_PI$L-10
538 0060 cd0000 call c_rtol
540 ; 158 if (Get_CurrentTargetSpeed() <= (35*Pole_Pair_Num*10) )
542 0063 cd0000 call _Get_CurrentTargetSpeed
544 0066 a0bd sub a,#189
545 0068 9f ld a,x
546 0069 a202 sbc a,#2
547 006b 2413 jruge L502
548 ; 159 DeltaVoltage_slip_s32 = ( Ki * SAMPLING_TIME_LowSpeed * (s32)Error_slip)/256;
550 006d b6fb ld a,_regul_PI$L-5
551 006f befa ld x,_regul_PI$L-6
552 0071 cd0000 call c_itol
554 0074 aee8 ld x,#_regul_PI$L-24
555 0076 cd0000 call c_rtol
557 0079 c60000 ld a,_Ki
558 007c ae14 ld x,#20
564 007e 2038 jra L702
565 0080 L502:
566 ; 160 else if(Get_CurrentTargetSpeed() > (35*Pole_Pair_Num*10)&&Get_CurrentTargetSpeed() < (80*Pole_Pair_Num*10) )
568 0080 cd0000 call _Get_CurrentTargetSpeed
570 0083 a0bd sub a,#189
571 0085 9f ld a,x
572 0086 a202 sbc a,#2
573 0088 251d jrult L112
575 008a cd0000 call _Get_CurrentTargetSpeed
577 008d a040 sub a,#64
578 008f 9f ld a,x
579 0090 a206 sbc a,#6
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -