📄 mc_pid_regulators.ls
字号:
732 01d3 fe ldw x,(x)
733 01d4 e708 ld (8,x),a
734 01d6 e709 ld (9,x),a
735 01d8 e707 ld (7,x),a
737 01da cc02b4 jp LC006
738 01dd L372:
739 ; 122 wIntegral_Term = PID_Struct->pPID_Var->hKi_Gain * wError;
741 01dd 1e2b ldw x,(OFST+7,sp)
742 01df fe ldw x,(x)
743 01e0 ee02 ldw x,(2,x)
744 01e2 cd0000 call c_itolx
746 01e5 96 ldw x,sp
747 01e6 1c0019 addw x,#OFST-11
748 01e9 cd0000 call c_lmul
750 01ec 96 ldw x,sp
751 01ed 1c001d addw x,#OFST-7
752 01f0 cd0000 call c_rtol
754 ; 123 wIntegral_sum_temp = PID_Struct->pPID_Var->wIntegral + wIntegral_Term;
756 01f3 1e2b ldw x,(OFST+7,sp)
757 01f5 fe ldw x,(x)
758 01f6 1c0006 addw x,#6
759 01f9 cd0000 call c_ltor
761 01fc 96 ldw x,sp
762 01fd 1c001d addw x,#OFST-7
763 0200 cd0000 call c_ladd
765 0203 96 ldw x,sp
766 0204 1c0021 addw x,#OFST-3
767 0207 cd0000 call c_rtol
769 ; 125 if (wIntegral_sum_temp > 0)
771 020a 96 ldw x,sp
772 020b 1c0021 addw x,#OFST-3
773 020e cd0000 call c_lzmp
775 0211 2d13 jrsle L772
776 ; 127 if (PID_Struct->pPID_Var->wIntegral < 0)
778 0213 1e2b ldw x,(OFST+7,sp)
779 0215 fe ldw x,(x)
780 0216 e606 ld a,(6,x)
781 0218 2a2a jrpl L503
782 ; 129 if (wIntegral_Term < 0)
784 021a 7b1d ld a,(OFST-7,sp)
785 021c 2a26 jrpl L503
786 ; 131 wIntegral_sum_temp = S32_MIN;
788 021e 5f clrw x
789 021f 1f23 ldw (OFST-1,sp),x
790 0221 ae8000 ldw x,#-32768
791 0224 201c jp LC005
792 0226 L772:
793 ; 137 if (PID_Struct->pPID_Var->wIntegral > 0)
795 0226 1e2b ldw x,(OFST+7,sp)
796 0228 fe ldw x,(x)
797 0229 1c0006 addw x,#6
798 022c cd0000 call c_lzmp
800 022f 2d13 jrsle L503
801 ; 139 if (wIntegral_Term > 0)
803 0231 96 ldw x,sp
804 0232 1c001d addw x,#OFST-7
805 0235 cd0000 call c_lzmp
807 0238 2d0a jrsle L503
808 ; 141 wIntegral_sum_temp = S32_MAX;
810 023a aeffff ldw x,#65535
811 023d 1f23 ldw (OFST-1,sp),x
812 023f ae7fff ldw x,#32767
813 0242 LC005:
814 0242 1f21 ldw (OFST-3,sp),x
815 0244 L503:
816 ; 146 if (wIntegral_sum_temp > PID_Struct->pPID_Const->wUpper_Limit_Integral)
818 0244 1e2b ldw x,(OFST+7,sp)
819 0246 ee02 ldw x,(2,x)
820 0248 1c0010 addw x,#16
821 024b cd0000 call c_ltor
823 024e 96 ldw x,sp
824 024f 1c0021 addw x,#OFST-3
825 0252 cd0000 call c_lcmp
827 0255 2e1e jrsge L313
828 ; 148 PID_Struct->pPID_Var->wIntegral = PID_Struct->pPID_Const->wUpper_Limit_Integral;
830 0257 1e2b ldw x,(OFST+7,sp)
831 0259 162b ldw y,(OFST+7,sp)
832 025b ee02 ldw x,(2,x)
833 025d 90fe ldw y,(y)
834 025f e613 ld a,(19,x)
835 0261 90e709 ld (9,y),a
836 0264 e612 ld a,(18,x)
837 0266 90e708 ld (8,y),a
838 0269 e611 ld a,(17,x)
839 026b 90e707 ld (7,y),a
840 026e e610 ld a,(16,x)
841 0270 LC007:
842 0270 90e706 ld (6,y),a
844 0273 2041 jra L572
845 0275 L313:
846 ; 150 else if (wIntegral_sum_temp < PID_Struct->pPID_Const->wLower_Limit_Integral)
848 0275 1e2b ldw x,(OFST+7,sp)
849 0277 ee02 ldw x,(2,x)
850 0279 1c000c addw x,#12
851 027c cd0000 call c_ltor
853 027f 96 ldw x,sp
854 0280 1c0021 addw x,#OFST-3
855 0283 cd0000 call c_lcmp
857 0286 2d1b jrsle L713
858 ; 152 PID_Struct->pPID_Var->wIntegral = PID_Struct->pPID_Const->wLower_Limit_Integral;
860 0288 1e2b ldw x,(OFST+7,sp)
861 028a 162b ldw y,(OFST+7,sp)
862 028c ee02 ldw x,(2,x)
863 028e 90fe ldw y,(y)
864 0290 e60f ld a,(15,x)
865 0292 90e709 ld (9,y),a
866 0295 e60e ld a,(14,x)
867 0297 90e708 ld (8,y),a
868 029a e60d ld a,(13,x)
869 029c 90e707 ld (7,y),a
870 029f e60c ld a,(12,x)
872 02a1 20cd jp LC007
873 02a3 L713:
874 ; 156 PID_Struct->pPID_Var->wIntegral = wIntegral_sum_temp;
876 02a3 1e2b ldw x,(OFST+7,sp)
877 02a5 fe ldw x,(x)
878 02a6 7b24 ld a,(OFST+0,sp)
879 02a8 e709 ld (9,x),a
880 02aa 7b23 ld a,(OFST-1,sp)
881 02ac e708 ld (8,x),a
882 02ae 7b22 ld a,(OFST-2,sp)
883 02b0 e707 ld (7,x),a
884 02b2 7b21 ld a,(OFST-3,sp)
885 02b4 LC006:
886 02b4 e706 ld (6,x),a
887 02b6 L572:
888 ; 163 wtemp = wError - PID_Struct->pPID_Var->wPreviousError;
890 02b6 96 ldw x,sp
891 02b7 1c0019 addw x,#OFST-11
892 02ba cd0000 call c_ltor
894 02bd 1e2b ldw x,(OFST+7,sp)
895 02bf fe ldw x,(x)
896 02c0 1c000a addw x,#10
897 02c3 cd0000 call c_lsub
899 02c6 96 ldw x,sp
900 02c7 1c001d addw x,#OFST-7
901 02ca cd0000 call c_rtol
903 ; 164 wDifferential_Term = PID_Struct->pPID_Var->hKd_Gain * wtemp;
905 02cd 1e2b ldw x,(OFST+7,sp)
906 02cf fe ldw x,(x)
907 02d0 ee04 ldw x,(4,x)
908 02d2 cd0000 call c_itolx
910 02d5 96 ldw x,sp
911 02d6 1c001d addw x,#OFST-7
912 02d9 cd0000 call c_lmul
914 02dc 96 ldw x,sp
915 02dd 1c001d addw x,#OFST-7
916 02e0 cd0000 call c_rtol
918 ; 165 PID_Struct->pPID_Var->wPreviousError = wError; // store value
920 02e3 1e2b ldw x,(OFST+7,sp)
921 02e5 fe ldw x,(x)
922 02e6 7b1c ld a,(OFST-8,sp)
923 02e8 e70d ld (13,x),a
924 02ea 7b1b ld a,(OFST-9,sp)
925 02ec e70c ld (12,x),a
926 02ee 7b1a ld a,(OFST-10,sp)
927 02f0 e70b ld (11,x),a
928 02f2 7b19 ld a,(OFST-11,sp)
929 02f4 e70a ld (10,x),a
930 ; 168 houtput_32 = (wProportional_Term/PID_Struct->pPID_Const->hKp_Divisor+
930 ; 169 PID_Struct->pPID_Var->wIntegral/PID_Struct->pPID_Const->hKi_Divisor +
930 ; 170 wDifferential_Term/PID_Struct->pPID_Const->hKd_Divisor);
932 02f6 1e2b ldw x,(OFST+7,sp)
933 02f8 ee02 ldw x,(2,x)
934 02fa ee06 ldw x,(6,x)
935 02fc cd0000 call c_uitolx
937 02ff 96 ldw x,sp
938 0300 1c0011 addw x,#OFST-19
939 0303 cd0000 call c_rtol
941 0306 96 ldw x,sp
942 0307 1c001d addw x,#OFST-7
943 030a cd0000 call c_ltor
945 030d 96 ldw x,sp
946 030e 1c0011 addw x,#OFST-19
947 0311 cd0000 call c_ldiv
949 0314 96 ldw x,sp
950 0315 1c000d addw x,#OFST-23
951 0318 cd0000 call c_rtol
953 031b 1e2b ldw x,(OFST+7,sp)
954 031d ee02 ldw x,(2,x)
955 031f ee04 ldw x,(4,x)
956 0321 cd0000 call c_uitolx
958 0324 96 ldw x,sp
959 0325 1c0009 addw x,#OFST-27
960 0328 cd0000 call c_rtol
962 032b 1e2b ldw x,(OFST+7,sp)
963 032d fe ldw x,(x)
964 032e 1c0006 addw x,#6
965 0331 cd0000 call c_ltor
967 0334 96 ldw x,sp
968 0335 1c0009 addw x,#OFST-27
969 0338 cd0000 call c_ldiv
971 033b 96 ldw x,sp
972 033c 1c0005 addw x,#OFST-31
973 033f cd0000 call c_rtol
975 0342 1e2b ldw x,(OFST+7,sp)
976 0344 ee02 ldw x,(2,x)
977 0346 ee02 ldw x,(2,x)
978 0348 cd0000 call c_uitolx
980 034b 96 ldw x,sp
981 034c 5c incw x
982 034d cd0000 call c_rtol
984 0350 96 ldw x,sp
985 0351 1c0015 addw x,#OFST-15
986 0354 cd0000 call c_ltor
988 0357 96 ldw x,sp
989 0358 5c incw x
990 0359 cd0000 call c_ldiv
992 035c 96 ldw x,sp
993 035d 1c0005 addw x,#OFST-31
994 0360 cd0000 call c_ladd
996 0363 96 ldw x,sp
997 0364 1c000d addw x,#OFST-23
998 0367 cd0000 call c_ladd
1000 036a 96 ldw x,sp
1001 036b 1c0021 addw x,#OFST-3
1002 036e cd0000 call c_rtol
1004 ; 172 if (houtput_32 > PID_Struct->pPID_Const->hUpper_Limit_Output)
1006 0371 1e2b ldw x,(OFST+7,sp)
1007 0373 ee02 ldw x,(2,x)
1008 0375 ee0a ldw x,(10,x)
1009 0377 cd0000 call c_itolx
1011 037a 96 ldw x,sp
1012 037b 1c0021 addw x,#OFST-3
1013 037e cd0000 call c_lcmp
1015 0381 2e08 jrsge L323
1016 ; 174 houtput_32 = PID_Struct->pPID_Const->hUpper_Limit_Output;
1018 0383 1e2b ldw x,(OFST+7,sp)
1019 0385 ee02 ldw x,(2,x)
1020 0387 ee0a ldw x,(10,x)
1024 0389 2018 jp LC008
1025 038b L323:
1026 ; 176 else if (houtput_32 < PID_Struct->pPID_Const->hLower_Limit_Output)
1028 038b 1e2b ldw x,(OFST+7,sp)
1029 038d ee02 ldw x,(2,x)
1030 038f ee08 ldw x,(8,x)
1031 0391 cd0000 call c_itolx
1033 0394 96 ldw x,sp
1034 0395 1c0021 addw x,#OFST-3
1035 0398 cd0000 call c_lcmp
1037 039b 2d10 jrsle L523
1038 ; 178 houtput_32 = PID_Struct->pPID_Const->hLower_Limit_Output;
1040 039d 1e2b ldw x,(OFST+7,sp)
1041 039f ee02 ldw x,(2,x)
1042 03a1 ee08 ldw x,(8,x)
1044 03a3 LC008:
1045 03a3 cd0000 call c_itolx
1046 03a6 96 ldw x,sp
1047 03a7 1c0021 addw x,#OFST-3
1048 03aa cd0000 call c_rtol
1050 03ad L523:
1051 ; 180 return((s16)(houtput_32));
1053 03ad 1e23 ldw x,(OFST-1,sp)
1056 03af 5b26 addw sp,#38
1057 03b1 81 ret
1070 xdef _PID_Regulator
1071 xdef _PI_Regulator
1072 xref.b c_x
1091 xref c_lsub
1092 xref c_ldiv
1093 xref c_uitolx
1094 xref c_lcmp
1095 xref c_lzmp
1096 xref c_ladd
1097 xref c_ltor
1098 xref c_lmul
1099 xref c_rtol
1100 xref c_itolx
1101 end
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -