智能小车控制程序

1、智能小车控制程序XDEF Entry ABSENTRY Entry INCLUDE PWM.incINCLUDE ECT.incINCLUDE PORT.incINCLUDE ATD.incROMStartEQU$4000INTCREQU$001EF_FINISHEQU$20XXTCNT EQU$0044C_LINEEQU$20XXC_POINTEQU$20XXP_SAVEEQU$20XXP_LINEEQU$20XXP_LINE2EQU$20XXP_ROWEQU$200AP_POSITIONEQU$200CP_YOUEQU$200EP_YOU1EQU$20XXP_COMPARE0 EQU$20

2、XXCOUNT10EQU$20XXTEMEQU$20XXP_LINE1EQU $2018LEFT_SEQU$201ARIGHT_SEQU$201BADD_NUM0EQU $201CADD_SUM0EQU$201ECOUNT_LEQU$2020AVERAGEEQU$2021TEM0EQU$2023ERROREQU$2025TCNT0EQU$2027TCNT1EQU$2029COMPARE0EQU$202BCOMPAREEQU$202CRIGHT1EQU$202DLEFT1EQU$202EMEQU$202FM0EQU$2030LINE_CHAEQU$2031POINT_CHAEQU$2032SUD

3、U01EQU $2034SUDU02EQU$2035SUDU03EQU$2036LIANXU_ERROR EQU $2037MINEQU$2038MAXEQU$2039E_HIGHEQU$203AP_POINTLEQU$203CP_POINTREQU$203DADD_SUMEQU$203EADD_NUMEQU$2040C_MINEQU$2042C_MAXEQU$2043TEM10EQU$2044FIRSTEQU$2046LASTEQU$2047FIRST_LINEEQU$2048LAST_LINEEQU$2049RESULT0 EQU $204ARESULT1EQU$204CRESULT2EQ

4、U$204ERESULT3EQU$2050RESULT4EQU$2052RESULT5EQU$2054DDEQU$2056DD1EQU$2057LINE_LINE_MAXLIANXUEQU $2058EQU $2059GAOSUEQU $205ADDDEQU $205BRESULT6EQU $205DRESULT7EQU $205F9 / 26STIME_COUNT EQU $2061STIME_COUNT0 EQU $2063FLOWFIRST0PERIOD1PERIOD2PERIODZHUANJIAOSUDUZHI_XIANEQU $2065EQU $2066EQU $2068EQU $2

5、06AEQU $206CEQU $206E EQU $2070EQU $2072WAN_XIANORG $2080CRRNT_INSEQU $2074RMB 2RMB $0ARMB 7RMB 1CRRNT_INS1RMB 2FUZ_INSFUZ_OUTSCOG_OUTRMB $0ARMB 7RMB 1FUZ_INS1FUZ_OUTS1COG_OUT1ORG $B600INPUT_MFSEQU *FCB $0001,$001A,$00ff,$000EFCB $000E,$002B,$000E,$000EFCB $001F,$003C,$000E,$000EFCB $0030,$004D,$000

6、E,$000EFCB $0041,$005C,$000E,$00ffFCB $0001,$0032,$00ff,$0006FCB $0019,$0055,$0006,$0006FCB $003C,$0078,$0006,$0006FCB $005F,$009B,$0006,$0006FCB $0082,$00b4,$0006,$00ffSGLTN_POS EQU *FCB 60,90,120,130,140,165,210INPUT_MFS1 FCB $0001,$0032,$00ff,$0006FCB $0019,$0055,$0006,$0006FCB $003C,$0078,$0006,

7、$0006FCB $005F,$009B,$0006,$0006FCB $0082,$00b4,$0006,$00ffFCB $0001,$0008,$00ff,$003fFCB $0004,$000e,$003f,$003fFCB $000a,$0014,$003f,$003fFCB $0010,$001a,$003f,$003fFCB $0016,$001F,$003f,$00ffSGLTN_POS1 EQUFCB 01,40,70,90,110,140,178EQURULE厲钐瘗睞枥庑赖矚慫润FDB FUZ_INS+$0003,FUZ_INS+$0005,$FFFE,FUZ_INS+$0

8、00c,$FFFF 矚慫润厲钐瘗睞枥庑赖賃。燴鐺險爱氇谴净聞創沟FDB FUZ_INS+$0003,FUZ_INS+$0006,$FFFE,FUZ_INS+$000d,$FFFF 聞創沟燴鐺險爱氇谴净祸。諍锩瀨濟溆塹籟残骛楼FDB FUZ_INS+$0003,FUZ_INS+$0007,$FFFE,FUZ_INS+$000e,$FFFF 残骛楼諍锩瀨濟溆塹籟婭。額閉镇桧猪訣锥酽锕极FDB FUZ_INS+$0003,FUZ_INS+$0008,$FFFE,FUZ_INS+$000f,$FFFF 酽锕极額閉镇桧猪訣锥顧。尔霁毙攬砖卤庑彈贸摄FDB FUZ_INS+$0003,FUZ_INS+$0

9、009,$FFFE,FUZ_INS+$0010,$FFFF 彈贸摄尔霁毙攬砖卤庑诒。箧飆鐸怼类蒋薔謀荞抟FDB FUZ_INS+$0004,FUZ_INS+$0005,$FFFE,FUZ_INS+$000d,$FFFF 謀荞抟箧飆鐸怼类蒋薔點。蹒骈時盡继價骚厦礴恳FDB FUZ_INS+$0004,FUZ_INS+$0006,$FFFE,FUZ_INS+$000e,$FFFF 厦礴恳蹒骈時盡继價骚卺。鳓鯡选块网羈泪茕桢广FDB FUZ_INS+$0004,FUZ_INS+$0007,$FFFE,FUZ_INS+$000f,$FFFF 茕桢广鳓鯡选块网羈泪镀。損鹌惨歷茏鴛賴鹅娅尽FDB FUZ_

10、INS+$0004,FUZ_INS+$0008,$FFFE,FUZ_INS+$0010,$FFFF 鹅娅尽損鹌惨歷茏鴛賴縈。FDB FUZ_INS+$0004,FUZ_INS+$0009,$FFFE,FUZ_INS+$0010,$FFFF羥为贍偾蛏练淨籟丛妈FDBFUZ_INS+$0002,FUZ_INS+$0005,$FFFE,FUZ_INS+$000b,$FFFF 籟丛妈羥为贍偾蛏练淨槠。鉉儐歲龈讶骅籴預頌圣FDB FUZ_INS+$0002,FUZ_INS+$0006,$FFFE,FUZ_INS+$000c,$FFFF 預頌圣鉉儐歲龈讶骅籴買。俨匀谔鱉调硯錦渗釤呛FDB FUZ_INS+

11、$0002,FUZ_INS+$0007,$FFFE,FUZ_INS+$000d,$FFFF 渗釤呛俨匀谔鱉调硯錦鋇。泻噦圣骋贶頂廡铙誅卧FDB FUZ_INS+$0002,FUZ_INS+$0008,$FFFE,FUZ_INS+$000e,$FFFF 铙誅卧泻噦圣骋贶頂廡缝。擁締凤袜FDB FUZ_INS+$0002,FUZ_INS+$0009,$FFFE,FUZ_INS+$000f,$FFFF 备訊顎轮烂蔷。擁締凤袜备訊顎轮烂蔷報。阃歲匱阊邺镓騷贓熱俣FDB FUZ_INS+$0001,FUZ_INS+$0005,$FFFE,FUZ_INS+$000a,$FFFF 贓熱俣阃歲匱阊邺镓騷鯛。囂

12、忏蒌鍥铃氈淚坛摶乡FDB FUZ_INS+$0001,FUZ_INS+$0006,$FFFE,FUZ_INS+$000b,$FFFF 坛摶乡囂忏蒌鍥铃氈淚跻。癟報伥铉锚鈰赘蜡變黲FDB FUZ_INS+$0001,FUZ_INS+$0007,$FFFE,FUZ_INS+$000c,$FFFF 蜡變黲癟報伥铉锚鈰赘籜。譖昙膚遙闫撷凄。FDB FUZ_INS+$0001,FUZ_INS+$0008,$FFFE,FUZ_INS+$000d,$FFFF 買鲷鴯譖昙膚遙闫撷凄届。買鲷鴯駕櫬鹕踪韦辚糴。FDB FUZ_INS+$0001,FUZ_INS+$0009,$FFFE,FUZ_INS+$000e,

13、$FFFF 綾镝鯛駕櫬鹕踪韦辚糴飙。綾镝鯛彦浃绥譎饴憂锦。FDB FUZ_INS+$0000,FUZ_INS+$0005,$FFFE,FUZ_INS+$000a,$FFFF 驅踬髏彦浃绥譎饴憂锦諑。驅踬髏绘燈鮒诛髅貺庑FDB FUZ_INS+$0000,FUZ_INS+$0006,$FFFE,FUZ_INS+$000a,$FFFF 猫虿驢绘燈鮒诛髅貺庑献。猫虿驢迳琐筆襖鸥娅薔FDB FUZ_INS+$0000,FUZ_INS+$0007,$FFFE,FUZ_INS+$000b,$FFFF 锹籁饗迳琐筆襖鸥娅薔嗚。锹籁饗黉碩饨荠龈话骛FDB FUZ_INS+$0000,FUZ_INS+$0008

14、,$FFFE,FUZ_INS+$000c,$FFFF 構氽頑黉碩饨荠龈话骛門。構氽頑檉簖疖網儂號泶FDB FUZ_INS+$0000,FUZ_INS+$0009,$FFFE,FUZ_INS+$000d,$FFFF 輒峄陽檉簖疖網儂號泶蛴。輒峄陽EQURULE1FDB FUZ_INS1+$0003,FUZ_INS1+$0005,$FFFE,FUZ_INS1+$0010,$FFFF 侧閆繭絳闕绚勵蜆贅。尧侧閆繭絳闕绚勵蜆贅瀝。FDB FUZ_INS1+$0003,FUZ_INS1+$0006,$FFFE,FUZ_INS1+$000F,$FFFF 饒鎂錕缢灩筧嚌俨淒。识饒鎂錕缢灩筧嚌俨淒侬。FDB

15、FUZ_INS1+$0003,FUZ_INS1+$0007,$FFFE,FUZ_INS1+$000E,$FFFF 鈹鋨劳臘锴痫婦胫籴。凍鈹鋨劳臘锴痫婦胫籴铍。FDB FUZ_INS1+$0003,FUZ_INS1+$0008,$FFFE,FUZ_INS1+$000E,$FFFF 諤銪灭萦欢煬鞏鹜錦。恥諤銪灭萦欢煬鞏鹜錦聰。FDB FUZ_INS1+$0003,FUZ_INS1+$0009,$FFFE,FUZ_INS1+$000E,$FFFF 腎鑰诎褳鉀沩懼統庫。鯊腎鑰诎褳鉀沩懼統庫摇。FDB FUZ_INS1+$0004,FUZ_INS1+$0005,$FFFE,FUZ_INS1+$0010,

16、$FFFF 癘鄴颃诌攆檸攜驤蔹。硕癘鄴颃诌攆檸攜驤蔹鸶。FDB FUZ_INS1+$0004,FUZ_INS1+$0006,$FFFE,FUZ_INS1+$0010,$FFFF 擻輳嬪諫迁择楨秘騖。阌擻輳嬪諫迁择楨秘騖輛。FDB FUZ_INS1+$0004,FUZ_INS1+$0007,$FFFE,FUZ_INS1+$000F,$FFFF 嚕躑竄贸恳彈瀘颔澩。氬嚕躑竄贸恳彈瀘颔澩纷。FDB FUZ_INS1+$0004,FUZ_INS1+$0008,$FFFE,FUZ_INS1+$000E,$FFFF 鹆資贏車贖孙滅獅赘。釷鹆資贏車贖孙滅獅赘慶。FDB FUZ_INS1+$0004,FUZ_

17、INS1+$0009,$FFFE,FUZ_INS1+$000E,$FFFF 阐譜鯪迳導嘯畫長凉。怂阐譜鯪迳導嘯畫長凉馴。FDB FUZ_INS1+$0002,FUZ_INS1+$0005,$FFFE,FUZ_INS1+$000D,$FFFF 辞調担鈧谄动禪泻類。谚辞調担鈧谄动禪泻類谨。FDB FUZ_INS1+$0002,FUZ_INS1+$0006,$FFFE,FUZ_INS1+$000D,$FFFF 觐詿缧铴嗫偽純铪锩。嘰觐詿缧铴嗫偽純铪锩癱。FDB FUZ_INS1+$0002,FUZ_INS1+$0007,$FFFE,FUZ_INS1+$000D,$FFFF 绐譏钲鏌觶鷹緇機库。熒绐譏

18、钲鏌觶鷹緇機库圆。FDB FUZ_INS1+$0002,FUZ_INS1+$0008,$FFFE,FUZ_INS1+$000D,$FFFF 渍螻偉阅劍鲰腎邏蘞。鶼渍螻偉阅劍鲰腎邏蘞阕。FDB FUZ_INS1+$0002,FUZ_INS1+$0009,$FFFE,FUZ_INS1+$000D,$FFFF 忧蔣氳頑莶驅藥悯骛。纣忧蔣氳頑莶驅藥悯骛覲。FDB FUZ_INS1+$0001,FUZ_INS1+$0005,$FFFE,FUZ_INS1+$000A,$FFFF 刍莖蛺饽亿顿裊赔泷。颖刍莖蛺饽亿顿裊赔泷涨。FDB FUZ_INS1+$0001,FUZ_INS1+$0006,$FFFE,FU

19、Z_INS1+$000B,$FFFF 驂膽閉驟羥闈詔寢賻。濫驂膽閉驟羥闈詔寢賻減。FDB FUZ_INS1+$0001,FUZ_INS1+$0007,$FFFE,FUZ_INS1+$000C,$FFFF 銻縵哜鳗鸿锓謎諏涼。銚銻縵哜鳗鸿锓謎諏涼鏗。FDB FUZ_INS1+$0001,FUZ_INS1+$0008,$FFFE,FUZ_INS1+$000C,$FFFF 貼綬电麥结鈺贖哓类。挤貼綬电麥结鈺贖哓类芈。FDB FUZ_INS1+$0001,FUZ_INS1+$0009,$FFFE,FUZ_INS1+$000C,$FFFF 荊紳谘侖驟辽輩袜錈。赔荊紳谘侖驟辽輩袜錈極。FDB FUZ_IN

20、S1+$0000,FUZ_INS1+$0005,$FFFE,FUZ_INS1+$000A,$FFFF 礙籟馐决穩賽釙冊庫。塤礙籟馐决穩賽釙冊庫麩。FDB FUZ_INS1+$0000,FUZ_INS1+$0006,$FFFE,FUZ_INS1+$000A,$FFFF 樣祕廬廂颤谚鍘羋蔺。裊樣祕廬廂颤谚鍘羋蔺递。FDB FUZ_INS1+$0000,FUZ_INS1+$0007,$FFFE,FUZ_INS1+$000B,$FFFF 嫗盤紲嘱珑詁鍬齊驁。仓嫗盤紲嘱珑詁鍬齊驁絛。FDB FUZ_INS1+$0000,FUZ_INS1+$0008,$FFFE,FUZ_INS1+$000C,$FFFF

21、萬璉轆娛閬蛏鬮绾瀧。绽萬璉轆娛閬蛏鬮绾瀧恒。FDB FUZ_INS1+$0000,FUZ_INS1+$0009,$FFFE,FUZ_INS1+$000C,$FFFF 顾燁鶚巯瀆蕪領鲡赙。骁顾燁鶚巯瀆蕪領鲡赙骠。WEIGHTEQUFCB $FF,$FF,$FF,$FF,$FFORG ROMStartEntryLDAA #$40STAA PWMCTLLDAA #$33STAA PWMPRCLKLDAA #$00STAA PWMCLKLDAA #$FFSTAA PWMPOLLDAA #$64STAA PWMPER0LDAA #$64STAA PWMPER1LDAA #$00STAA PWMDTY0L

22、DAA #$00STAA PWMDTY1LDAA #$EASTAA PWMPER4LDAA #$60STAA PWMPER5LDAA #$11STAA PWMDTY4LDAA #$94STAA PWMDTY5LDAA #$33STAA PWMELDAA #$0FSTAA TIOSLDAA #$00STAA TIELDAA #$03STAA TSCR2LDAA #$40STAA TCTL3LDAA #$81STAA TSCR1LDAA #$00STAA ATD0CTL2LDAA #$08STAA ATD0CTL3LDAA #$80STAA ATD0CTL4LDAA #$00STAA ATD0DI

23、ENLDAA #$00STAA ATD1CTL2LDAA #$08STAA ATD1CTL3LDAA #$80STAA ATD1CTL4LDAA #$00STAA ATD1DIENSTAA DDRHLDAA #$00STAA DDRBLDAA #$FFSTAA DDRKLDAA #$08STAA PORTKLDAA #$00STAA INTCRLDAA #$00STAA M0LDAA #$2DSTAA DDLDD #$1173STD RESULT0LDD #$10D0STD RESULT6LDD #$1200STD RESULT7LDAA PORTBSTAA LIANXUCMPA #$FEBE

24、Q SUDU1CMPA #$F7BEQ SUDU2CMPA #$F6BEQ SUDU3BRA SUDU013 / 26SUDU0LDAA #$40STAA GAOSULDD #$14A0STD ZHI_XIANLDD #$16A0STD WAN_XIANLDAA #$42STAA SUDU01LDAA #$41STAA SUDU02LDAA #$41LBRA AGAIN31 / 26SUDU1LDAA #$41STAA GAOSULDD #$1400STD ZHI_XIANLDD #$1630STD WAN_XIANLDAA #$42STAA SUDU01LDAA #$41STAA SUDU0

25、2LDAA #$41STAA SUDU03BRA AGAINSUDU2LDAA #$42STAA GAOSULDD #$1350STD ZHIXIANLDD #$16A0STD WAN_XIANLDAA #$42STAA SUDU01LDAA #$42STAA SUDU02LDAA #$41STAA SUDU03BRA AGAINSUDU3LDAA #$42STAA GAOSULDD #$12A0STD ZHI_XIANLDD #$16A0STD WAN_XIANLDAA #$42STAA SUDU01LDAA #$42STAA SUDU02LDAA #$41STAA SUDU03AGAINW

26、F_FBRA AGAINLDAA PORTHANDA #$02CMPA #$02BEQ WF_FCLILDAA #$00STAA C_LINESTAA F_FINISHLDD #$2100STD P_SAVELDD TCNTADDD #$0EE0STD TC0LDAA #$FFSTAA TFLG1LDAA #$01STAA TIEWF_FINISHLDAA F_FINISHCMPA #$01BNE WF_FINISHSEILDD #$2C3FSTD P_LINESTD P_ROWLDD #$3159STD P_YOU1LDD #$2E00STD P_POSITIONLDD #$3100STD

27、P_LINE1NEXT_POINT0LDX P_LINE1LDY P_LINELDAA YSTAA XINXLDD P_LINESUBD #$0020STD P_LINECPD #$2100BHI NEXT_POINT0NEXT_LINE0LDD P_ROWSUBD #$0001STD P_ROWSTD P_LINECPD #$2C21BHI NEXT_POINT0LDX #$3268LDAA XSTAA C_MINSTAA C_MAXJUXINXLDAA XCMPA C_MINBHI MAX1STAA C_MINMAX1CMPA C_MAXBLO MIN1STAA C_MAXMIN1CPX

28、#$3B32BGT FINISH2BRA JUXFINISH2LDAA #$00LDAB C_MINSTD TEM10LDAA #$00LDAB C_MAXADDD TEM10LDX #$0002IDIVTFR X,DSUBD #$0010STAB COMPARELDD #$3100STD P_LINE2LDAA #$05STAA P_POINTLLDAA #$55STAA P_POINTRNEXT_LINELDX P_LINE2LDAB P_POINTRABXSTX E_HIGHLDX P_LINE2LDAB P_POINTLABXNEXT_POINTLDAA XCMPA COMPARELB

29、HI N_BLACK1TFR X,DSUBD P_LINE2STAB LEFT_SN_WHITEINXCPX E_HIGHBGT LINE_FULLLDAA XCMPA COMPAREBLO N_WHITEN_WHITE1TFR X,DSUBD P_LINE2STAB RIGHT_SLDX E_HIGHRIGHTLDAA XCMPA COMPAREBHI N_BLACK2TFR X,DSUBD P_LINE2CMPB RIGHT_SLBLO HUANFINISH_0SUBB RIGHT_SCMPB #$0ALINE_FULLLBGT LINE_ERRORTFR X,DSUBD P_LINE2S

30、TAB RIGHT_SADDB LEFT_SLSRBLDY P_POSITIONSTAB YCMPB #$10BLS L_OVERFLOWCMPB #$49BGE R_OVERFLOWSUBB #$10STAB P_POINTLADDB #$20STAB P_POINTRBRA INC_LINEL_OVERFLOWTBAADDA #$10STAA P_POINTRLDAA #$01STAA P_POINTLBRA INC_LINER_OVERFLOWTBASUBA #$10STAA P_POINTLLDAA #$58STAA P_POINTRBRA INC_LINEINC_LINELDD P_

31、LINE2ADDD #$005ASTD P_LINE2CPD #$3B32BHI FIND_FINISHLDX P_POSITIONINXSTX P_POSITIONLBRA NEXT_LINEN_BLACK1INXCPX E_HIGHBGT LINE_ERRORLBRA NEXT_POINTN_BLACK2DEXCPX P_LINE2BLT LINE_ERRORLBRA RIGHTLINE_ERRORLDAA #$FFLDY P_POSITIONSTAA YHUANBRA INC_LINESTAB TEMLDAB RIGHT_SLDAA TEMSTAA RIGHT_SLBRA FINISH_

32、0FIND_FINISHSSLDX #$2E00LDAA #$00STAA ERRORAGAIN00LDAB XCPX #$2E1ELBEQ HINXCMPB #$FFBNE AGAIN00INC ERRORBRA AGAIN00LDAA ERRORCMPA #$18LBHI FIGURE_ERRORLDAA #$1ESTAA LAST_LINELDX #$2E1DCHA0LDAA XCMPA #$FFBEQ AGAIN002STAA LASTLDAA #$01STAA FIRST_LINELDX #$2DFFCHA1INXLDAA XCMPA #$FFBEQ AGAIN000STAA MIN

33、STAA MAXSTAA FIRSTCHA2INXLDAA XCMPA #$FFBEQ AGAIN001CMPA MAXBLT MIN0STAA MAXMIN0CMPA MINBGT MAX0STAA MINMAX0CPX #$2E1DBGT FINISH1BRA CHA2AGAIN000INC FIRST_LINECPX #$2E1DLBEQ AGAINBRA CHA1AGAIN001CPX #$2E1DBEQ FINISH1BRA CHA2AGAIN002DEC LAST_LINEDEXCPX #$2E00LBLT AGAINBRA CHA0FINISH1LDAA LASTLDAB FIR

34、STSBASTAA POINT_CHALDAA POINT_CHAADDA #$5ASTAA POINT_CHALDAA LAST_LINELDAB FIRST_LINESBASTAA LINE_CHALDAA POINT_CHAADDA #$01STAA CRRNT_INS1LDAA LINE_CHAADDA #$01STAA CRRNT_INS1+1FUZZIFY1:LDX #INPUT_MFS1LDY #FUZ_INS1LDAA CRRNT_INS1LDAB #5GRAD_LOOP1: MEMDBNE B,GRAD_LOOP1LDAA CRRNT_INS1+1LDAB #5GRAD_LP

35、1: MEMDBNE B,GRAD_LP1LDAB #07RULE_EV AL1: CLR 1,Y+DBNE B,RULE_EV AL1LDX #RULE1LDY #WEIGHTLDAB #25EVALUE1:LDAA #$FFREVWDBNE B,EV ALUE1DEFUZ1:LDX #SGLTN_POS1LDY #FUZ_OUTS1LDAB #07WAVEDIVTFR Y,DSTAB COG_OUT1LDAA LASTADDA #$01STAA CRRNT_INSLDAA COG_OUT1ADDA #$01STAA CRRNT_INS+1FUZZIFY:LDX #INPUT_MFSLDY

36、#FUZ_INSLDAA CRRNT_INSLDAB #5GRAD_LOOP: MEMDBNE B,GRAD_LOOPLDAA CRRNT_INS+1LDAB #5GRAD_LP:MEMDBNE B,GRAD_LPLDAB #07RULE_EV AL:CLR 1,Y+DBNE B,RULE_EV ALLDX #RULELDY #WEIGHTLDAB #25EVALUE:LDAA #$FFREVWDBNE B,EV ALUEDEFUZ:LDX #SGLTN_POSLDY #FUZ_OUTSLDAB #07WAVEDIVTFR Y,DSTAB COG_OUTLDAA #$0ALDAB COG_OU

37、TMULADDD #$0C26STD RESULT1CPD RESULT6BLT XIAOCPD RESULT7BGT DAZHUAN LDD RESULT1CPD #$14A0BGT GUOCPD #$0E00BLT GAOSTAA PWMDTY4STAB PWMDTY5STD RESULT0LBRA MOTORGUOLDD #$14A0STAA PWMDTY4STAB PWMDTY5STD RESULT0LBRA MOTORGAOLDD #$0E03STAA PWMDTY4STAB PWMDTY5STD RESULT0LBRA MOTORXIAOLDD RESULT0CPD RESULT7

38、BGT SBRA ZHUANDALDD RESULT0CPD RESULT6BLT SBRA ZHUANLDD RESULT0ADDD RESULT1LDX #$0002IDIVTFR X,DCPD #$14AFBGT GUO0CPD #$0E00BLO GAI0STAA PWMDTY4STAB PWMDTY5STD RESULT0STD RESULT4JMP MOTORGUO0LDD #$14AFSTD RESULT0STD RESULT4STAA PWMDTY4STAB PWMDTY5BRA MOTORGAI0LDD #$0E00STAA PWMDTY4STAB PWMDTY5STD RE

39、SULT0STD RESULT4STAA PWMDTY4STAB PWMDTY5BRA MOTORFIGURE_ERRORLDAA #$AA35 / 26STAA PORTBLDAA #$41STAA PWMDTY0LDAA #$00STAA PWMDTY1LDD RESULT0CPD #$1194BHI FIGURE_ERROR1LDD RESULT0SUBD #$0064CPD #$0E00BLO GAI1STAA PWMDTY4STAB PWMDTY5STD RESULT0LDAA #$2DSTAA DDGAI1JMP AGAINLDD #$0E00STAA PWMDTY4STAB PW

40、MDTY5STD RESULT0LDAA #$2DSTAA DDJMP AGAINFIGURE_ERROR1LDD RESULT0ADDD #$0064CPD #$14AFBHI GUO1STAA PWMDTY4STAB PWMDTY5STD RESULT0LDAA #$2DSTAA DDJMP AGAINGUO1LDD #$14AFSTD RESULT0JMP AGAINMOTORB0LDD #$0000LDAA #$80STAB PWMDTY5LDAA #$2DSTAA DDLDAA COG_OUT1CMPA #$51LBLT B1CMPA #$64LBGT B1BRA B0INC M0S

41、TD STIME_COUNTSTD STIME_COUNT0STAA TFLG1CE0LDAA TFLG1ANDA #$80CMPA #$80BEQ CAMPURE0LDD STIME_COUNT0ADDD #$0001STD STIME_COUNT0LDD STIME_COUNT0 CPD ZHI_XIANBNE CE0BRA MANCAMPURE0LDD TC7STD FIRST0LDAA #$80STAA TFLG1CELDAA TFLG1ANDA #$80CMPA #$80BEQ KUAILDD STIME_COUNTADDD #$0001STD STIME_COUNTLDD STIM

42、E_COUNTCPD ZHI_XIAN37 / 26BNE CEBRA MANMANLDAA #$00STAA SUDUBRA XIELVKUAILDAA #$01STAA SUDUBRA XIELVXIELVLDAA SUDUCMPA #$01BEQ CHULILDAA GAOSUSTAA PWMDTY0LDAA #$00STAA PWMDTY1LBRA ANGLE00CHULILDAA #$00STAA PWMDTY0LDAA #$00STAA PWMDTY1CALL STIMELDAA #$00STAA M0LBRA ANGLE00B1LDAA M0CMPA #$03BLT BUBRA

43、XIANBULDAA SUDU02STAA PWMDTY0LDAA #$00STAA PWMDTY1LDAA #$00STAA M0LBRA ANGLE00XIANLDD #$0000STD STIME_COUNTSTD STIME_COUNT0LDAA SUDUSTAA TFLG140 / 26CE01LDAA TFLG1ANDA #$80CMPA #$80BEQ CAMPURE00LDD STIME_COUNT0ADDD #$0001STD STIME_COUNT0LDD STIME_COUNT0CPD WAN_XIANBNE CE01LBRA MANCAMPURE00LDAA #$80STAA TFLG1CE00LDAA TFLG1ANDA #$80CMPA #$80BEQ KUAI0LDD STIME_COUNTADDD