智能小车控制程序

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

2、_YOUEQU$200EP_YOU1EQU$20XXP_COMPARE0 EQU$20XXCOUNT10EQU$20XXTEMEQU$20XXP_LINE1EQU $2018LEFT_SEQU$201ARIGHT_SEQU$201BADD_NUM0EQU $201CADD_SUM0EQU$201ECOUNT_LEQU$2020AVERAGEEQU$2021TEM0EQU$2023ERROREQU$2025TCNT0EQU$2027TCNT1EQU$2029COMPARE0EQU$202BCOMPAREEQU$202CRIGHT1EQU$202DLEFT1EQU$202EMEQU$202FM0E

3、QU$2030LINE_CHAEQU$2031POINT_CHAEQU$2032SUDU01EQU $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$20

4、49RESULT0EQU $204ARESULT1EQU$204CRESULT2EQU$204ERESULT3EQU$2050RESULT4EQU$2052RESULT5EQU$2054DDEQU$2056DD1EQU$2057LINE_LINE_MAX EQU $2058LIANXUEQU$2059GAOSUEQU$205ADDDEQU $205BRESULT6EQU$205DRESULT7EQU$205FSTIME_COUNT EQU $2061STIME_COUNT0 EQU $2063FLOWEQU $2065FIRST0EQU $2066PERIOD1 EQU $2068 PERIO

5、D2 EQU $206A PERIOD EQU $206C ZHUANJIAO EQU $206E SUDU EQU $2070 ZHI_XIANEQU $2072WAN_XIANEQU $2074ORG $2080 CRRNT_INS RMB 2 FUZ_INS RMB $0A FUZ_OUTS RMB 7 COG_OUT RMB 1 CRRNT_INS1 RMB 2 FUZ_INS1 RMB $0A FUZ_OUTS1 RMB 7 COG_OUT1 RMB 1 ORG $B600 INPUT_MFS EQU * FCB $0001,$001A,$00ff,$000E FCB $000E,$

6、002B,$000E,$000E FCB $001F,$003C,$000E,$000E FCB $0030,$004D,$000E,$000E FCB $0041,$005C,$000E,$00ff FCB $0001,$0032,$00ff,$0006 FCB $0019,$0055,$0006,$0006 FCB $003C,$0078,$0006,$0006 FCB $005F,$009B,$0006,$0006 FCB $0082,$00b4,$0006,$00ff SGLTN_POS EQU *FCB 60,90,120,130,140,165,210 INPUT_MFS1 FCB

7、 $0001,$0032,$00ff,$0006 FCB $0019,$0055,$0006,$0006 FCB $003C,$0078,$0006,$0006 FCB $005F,$009B,$0006,$0006 FCB $0082,$00b4,$0006,$00ff FCB $0001,$0008,$00ff,$003f FCB $0004,$000e,$003f,$003f FCB $000a,$0014,$003f,$003fFCB $0010,$001a,$003f,$003fFCB $0016,$001F,$003f,$00ffSGLTN_POS1 EQU * FCB 01,40

8、,70,90,110,140,178RULE EQU *FDB FUZ_INS+$0003,FUZ_INS+$0005,$FFFE,FUZ_INS+$000c,$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+

9、$0008,$FFFE,FUZ_INS+$000f,$FFFF 酽锕极 額閉镇桧猪訣锥。酽锕极額閉镇桧猪訣锥顧。FDB FUZ_INS+$0003,FUZ_INS+$0009,$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 厦礴恳 蹒骈時盡继價骚。厦礴恳蹒骈時盡继價骚

10、卺。FDB FUZ_INS+$0004,FUZ_INS+$0007,$FFFE,FUZ_INS+$000f,$FFFF 茕桢广 鳓鯡选块网羈泪。茕桢广鳓鯡选块网羈泪镀。FDB FUZ_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 籟 丛 妈 羥为贍偾蛏练淨。籟丛妈羥为

11、贍偾蛏练淨槠。FDB FUZ_INS+$0002,FUZ_INS+$0006,$FFFE,FUZ_INS+$000c,$FFFF 預頌圣 鉉儐歲龈讶骅籴。預頌圣鉉儐歲龈讶骅籴買。FDB FUZ_INS+$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+$

12、000f,$FFFF 擁締凤袜 备訊顎轮烂蔷。擁締凤袜备訊顎轮烂蔷報。FDB FUZ_INS+$0001,FUZ_INS+$0005,$FFFE,FUZ_INS+$000a,$FFFF 贓熱俣 阃歲匱阊邺镓騷。贓熱俣阃歲匱阊邺镓騷鯛。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,F

13、UZ_INS+$0008,$FFFE,FUZ_INS+$000d,$FFFF 買鲷鴯 譖昙膚遙闫撷凄。買鲷鴯譖昙膚遙闫撷凄届。FDB FUZ_INS+$0001,FUZ_INS+$0009,$FFFE,FUZ_INS+$000e,$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 猫虿驢 绘燈鮒诛髅貺庑。猫虿驢

14、绘燈鮒诛髅貺庑献。FDB FUZ_INS+$0000,FUZ_INS+$0007,$FFFE,FUZ_INS+$000b,$FFFF 锹籁饗 迳琐筆襖鸥娅薔。锹籁饗迳琐筆襖鸥娅薔嗚。FDB FUZ_INS+$0000,FUZ_INS+$0008,$FFFE,FUZ_INS+$000c,$FFFF 構氽頑 黉碩饨荠龈话骛。構氽頑黉碩饨荠龈话骛門。FDB FUZ_INS+$0000,FUZ_INS+$0009,$FFFE,FUZ_INS+$000d,$FFFF 輒峄陽 檉簖疖網儂號泶。輒峄陽檉簖疖網儂號泶蛴。RULE1 EQU *FDB FUZ_INS1+$0003,FUZ_INS1+$0005,

15、$FFFE,FUZ_INS1+$0010,$FFFF 尧 侧閆繭絳闕绚勵蜆贅。尧侧閆繭絳闕绚勵蜆贅瀝。FDB FUZ_INS1+$0003,FUZ_INS1+$0006,$FFFE,FUZ_INS1+$000F,$FFFF 识 饒鎂錕缢灩筧嚌俨淒。识饒鎂錕缢灩筧嚌俨淒侬。FDB FUZ_INS1+$0003,FUZ_INS1+$0007,$FFFE,FUZ_INS1+$000E,$FFFF 凍 鈹鋨劳臘锴痫婦胫籴。凍鈹鋨劳臘锴痫婦胫籴铍。FDB FUZ_INS1+$0003,FUZ_INS1+$0008,$FFFE,FUZ_INS1+$000E,$FFFF 恥 諤銪灭萦欢煬鞏鹜錦。恥諤銪灭萦欢

16、煬鞏鹜錦聰。FDB FUZ_INS1+$0003,FUZ_INS1+$0009,$FFFE,FUZ_INS1+$000E,$FFFF 鯊 腎鑰诎褳鉀沩懼統庫。鯊腎鑰诎褳鉀沩懼統庫摇。FDB FUZ_INS1+$0004,FUZ_INS1+$0005,$FFFE,FUZ_INS1+$0010,$FFFF 硕 癘鄴颃诌攆檸攜驤蔹。硕癘鄴颃诌攆檸攜驤蔹鸶。FDB FUZ_INS1+$0004,FUZ_INS1+$0006,$FFFE,FUZ_INS1+$0010,$FFFF 阌 擻輳嬪諫迁择楨秘騖。阌擻輳嬪諫迁择楨秘騖輛。FDB FUZ_INS1+$0004,FUZ_INS1+$0007,$FFFE

17、,FUZ_INS1+$000F,$FFFF 氬 嚕躑竄贸恳彈瀘颔澩。氬嚕躑竄贸恳彈瀘颔澩纷。FDB FUZ_INS1+$0004,FUZ_INS1+$0008,$FFFE,FUZ_INS1+$000E,$FFFF 釷 鹆資贏車贖孙滅獅赘。釷鹆資贏車贖孙滅獅赘慶。FDB FUZ_INS1+$0004,FUZ_INS1+$0009,$FFFE,FUZ_INS1+$000E,$FFFF 怂 阐譜鯪迳導嘯畫長凉。怂阐譜鯪迳導嘯畫長凉馴。FDB FUZ_INS1+$0002,FUZ_INS1+$0005,$FFFE,FUZ_INS1+$000D,$FFFF 谚 辞調担鈧谄动禪泻類。谚辞調担鈧谄动禪泻類谨

18、。FDB FUZ_INS1+$0002,FUZ_INS1+$0006,$FFFE,FUZ_INS1+$000D,$FFFF 嘰 觐詿缧铴嗫偽純铪锩。嘰觐詿缧铴嗫偽純铪锩癱。FDB FUZ_INS1+$0002,FUZ_INS1+$0007,$FFFE,FUZ_INS1+$000D,$FFFF 熒 绐譏钲鏌觶鷹緇機库。熒绐譏钲鏌觶鷹緇機库圆。FDB FUZ_INS1+$0002,FUZ_INS1+$0008,$FFFE,FUZ_INS1+$000D,$FFFF 鶼 渍螻偉阅劍鲰腎邏蘞。鶼渍螻偉阅劍鲰腎邏蘞阕。FDB FUZ_INS1+$0002,FUZ_INS1+$0009,$FFFE,FUZ_

19、INS1+$000D,$FFFF 纣 忧蔣氳頑莶驅藥悯骛。纣忧蔣氳頑莶驅藥悯骛覲。FDB FUZ_INS1+$0001,FUZ_INS1+$0005,$FFFE,FUZ_INS1+$000A,$FFFF 颖 刍莖蛺饽亿顿裊赔泷。颖刍莖蛺饽亿顿裊赔泷涨。FDB FUZ_INS1+$0001,FUZ_INS1+$0006,$FFFE,FUZ_INS1+$000B,$FFFF 濫 驂膽閉驟羥闈詔寢賻。濫驂膽閉驟羥闈詔寢賻減。FDB FUZ_INS1+$0001,FUZ_INS1+$0007,$FFFE,FUZ_INS1+$000C,$FFFF 銚 銻縵哜鳗鸿锓謎諏涼。銚銻縵哜鳗鸿锓謎諏涼鏗。FDB

20、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_INS1+$0000,FUZ_INS1+$0005,$FFFE,FUZ_INS1+$000A,$FFFF 塤 礙籟馐决穩賽釙冊庫。塤礙籟馐决穩賽釙冊庫麩。FDB FUZ_INS1+$0000,FUZ_INS1+$0006,$FFFE,FUZ_INS1+

21、$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 绽 萬璉轆娛閬蛏鬮绾瀧。绽萬璉轆娛閬蛏鬮绾瀧恒。FDB FUZ_INS1+$0000,FUZ_INS1+$0009,$FFFE,FUZ_INS1+$000C,$FFFF 骁 顾燁鶚巯瀆蕪領鲡赙。骁顾燁鶚巯瀆蕪領鲡赙骠。WEIGHT EQ

22、U *FCB $FF,$FF,$FF,$FF,$FFORG ROMStartEntryLDAA #$40STAA PWMCTLLDAA #$33STAA PWMPRCLKLDAA #$00STAA PWMCLKLDAA #$FFSTAA PWMPOLLDAA #$64STAA PWMPER0LDAA #$64STAA PWMPER1LDAA #$00STAA PWMDTY0LDAA #$00STAA PWMDTY1LDAA #$EASTAA PWMPER4LDAA #$60STAA PWMPER5LDAA #$11STAA PWMDTY4LDAA #$94STAA PWMDTY5LDAA #$

23、33STAA PWMELDAA #$0FSTAA TIOSLDAA #$00STAA TIELDAA #$03STAA TSCR2LDAA #$40STAA TCTL3LDAA #$81STAA TSCR1LDAA #$00STAA ATD0CTL2LDAA #$08STAA ATD0CTL3LDAA #$80STAA ATD0CTL4LDAA #$00STAA ATD0DIENLDAA #$00STAA ATD1CTL2LDAA #$08STAA ATD1CTL3LDAA #$80STAA ATD1CTL4LDAA #$00STAA ATD1DIENLDAA #$00STAA DDRHLDA

24、A #$00STAA DDRBLDAA #$FFSTAA DDRKLDAA #$08STAA PORTKLDAA #$00STAA INTCRLDAA #$00STAA M0LDAA #$2DSTAA DDLDD #$1173STD RESULT0LDD #$10D0STD RESULT6 LDD #$1200 STD RESULT7 LDAA PORTB STAA LIANXU CMPA #$FE BEQ SUDU1 CMPA #$F7 BEQ SUDU2 CMPA #$F6 BEQ SUDU3 BRA SUDU0 SUDU0LDAA #$40STAA GAOSULDD #$14A0STD

25、ZHI_XIANLDD #$16A0STD WAN_XIANLDAA #$42STAA SUDU01LDAA #$41STAA SUDU02LDAA #$41SUDU1SUDU2STD ZHISUDU3STAA SUDU03 LBRA AGAIN LDAA #$41 STAA GAOSU LDD #$1400 STD ZHI_XIAN LDD #$1630 STD WAN_XIANLDAA #$42STAA SUDU01 LDAA #$41 STAA SUDU02 LDAA #$41 STAA SUDU03 BRA AGAIN LDAA #$42 STAA GAOSU LDD #$1350 X

26、IANLDD #$16A0STD WAN_XIAN LDAA #$42 STAA SUDU01 LDAA #$42 STAA SUDU02 LDAA #$41 STAA SUDU03 BRA AGAIN LDAA #$42 STAA GAOSU LDD #$12A0 STD ZHI_XIAN LDD #$16A0 STD WAN_XIANLDAA #$42STAA SUDU01 LDAA #$42 STAA SUDU02 LDAA #$41 STAA SUDU03BRA AGAINAGAIN WF_FLDAA PORTHANDA #$02CMPA #$02 BEQ WF_F CLI LDAA

27、#$00 STAA C_LINE STAA F_FINISH LDD #$2100 STD P_SAVE LDD TCNT ADDD #$0EE0 STD TC0 LDAA #$FF STAA TFLG1 LDAA #$01 STAA TIE WF_FINISH LDAA F_FINISHCMPA #$01BNE WF_FINISH SEI LDD #$2C3FSTD P_LINE STD P_ROW LDD #$3159 STD P_YOU1 LDD #$2E00 STD P_POSITION LDD #$3100 STD P_LINE1 LDX P_LINE1 NEXT_POINT0 LDY P_LINE LDAA Y STAA X INX LDD P_LINE SUB