.module demoday.c 0000 .dbfile demoday.c .area data 0000 _pwm_vals:: 0000 .blkb 2 .area idata 0000 0000 .word 0 .area data 0002 .blkb 2 .area idata 0002 4268 .word 17000 .area data 0004 .blkb 2 .area idata 0004 42CC .word 17100 .area data 0006 .blkb 2 .area idata 0006 4330 .word 17200 .area data 0008 .blkb 2 .area idata 0008 4394 .word 17300 .area data 000A .blkb 2 .area idata 000A 43F8 .word 17400 .area data 000C .blkb 2 .area idata 000C 445C .word 17500 .area data 000E .blkb 2 .area idata 000E 44C0 .word 17600 .area data 0010 .blkb 2 .area idata 0010 4524 .word 17700 .area data 0012 .blkb 2 .area idata 0012 4588 .word 17800 .area data 0014 .blkb 2 .area idata 0014 45EC .word 17900 .area data 0016 .blkb 2 .area idata 0016 4650 .word 18000 .area data 0018 .blkb 2 .area idata 0018 46B4 .word 18100 .area data 001A .blkb 2 .area idata 001A 4718 .word 18200 .area data 001C .blkb 2 .area idata 001C 477C .word 18300 .area data 001E .blkb 2 .area idata 001E 47E0 .word 18400 .area data 0020 .blkb 2 .area idata 0020 4844 .word 18500 .area data 0022 .blkb 2 .area idata 0022 48A8 .word 18600 .area data 0024 .blkb 2 .area idata 0024 490C .word 18700 .area data 0026 .blkb 2 .area idata 0026 4970 .word 18800 .area data 0028 .blkb 2 .area idata 0028 49D4 .word 18900 .area data 002A .blkb 2 .area idata 002A 4A38 .word 19000 .area data 002C .blkb 2 .area idata 002C 4A9C .word 19100 .area data 002E .blkb 2 .area idata 002E 4B00 .word 19200 .area data 0030 .blkb 2 .area idata 0030 4B64 .word 19300 .area data 0032 .blkb 2 .area idata 0032 4BC8 .word 19400 .area data 0034 .blkb 2 .area idata 0034 4C2C .word 19500 .area data 0036 .blkb 2 .area idata 0036 4C90 .word 19600 .area data 0038 .blkb 2 .area idata 0038 4CF4 .word 19700 .area data 003A .blkb 2 .area idata 003A 4D58 .word 19800 .area data 003C .blkb 2 .area idata 003C 4DBC .word 19900 .area data 003E .blkb 2 .area idata 003E 4E20 .word 20000 .area data 0040 .blkb 2 .area idata 0040 4E84 .word 20100 .area data 0042 .blkb 2 .area idata 0042 4EE8 .word 20200 .area data 0044 .blkb 2 .area idata 0044 4F4C .word 20300 .area data 0046 .blkb 2 .area idata 0046 4FB0 .word 20400 .area data 0048 .blkb 2 .area idata 0048 5014 .word 20500 .area data 004A .blkb 2 .area idata 004A 5078 .word 20600 .area data 004C .blkb 2 .area idata 004C 50DC .word 20700 .area data 004E .blkb 2 .area idata 004E 5140 .word 20800 .area data 0050 .blkb 2 .area idata 0050 51A4 .word 20900 .area data 0052 .blkb 2 .area idata 0052 5208 .word 21000 .area data 0054 .blkb 2 .area idata 0054 526C .word 21100 .area data 0056 .blkb 2 .area idata 0056 52D0 .word 21200 .area data 0058 .blkb 2 .area idata 0058 5334 .word 21300 .area data 005A .blkb 2 .area idata 005A 5398 .word 21400 .area data 005C .blkb 2 .area idata 005C 53FC .word 21500 .area data 005E .blkb 2 .area idata 005E 5460 .word 21600 .area data 0060 .blkb 2 .area idata 0060 54C4 .word 21700 .area data 0062 .blkb 2 .area idata 0062 5528 .word 21800 .area data 0064 .blkb 2 .area idata 0064 558C .word 21900 .area data 0066 .blkb 2 .area idata 0066 55F0 .word 22000 .area data 0068 .blkb 2 .area idata 0068 5654 .word 22100 .area data 006A .blkb 2 .area idata 006A 56B8 .word 22200 .area data 006C .blkb 2 .area idata 006C 571C .word 22300 .area data 006E .blkb 2 .area idata 006E 5780 .word 22400 .area data 0070 .blkb 2 .area idata 0070 57E4 .word 22500 .area data 0072 .blkb 2 .area idata 0072 5848 .word 22600 .area data 0074 .blkb 2 .area idata 0074 58AC .word 22700 .area data 0076 .blkb 2 .area idata 0076 5910 .word 22800 .area data 0078 .blkb 2 .area idata 0078 5974 .word 22900 .area data 007A .blkb 2 .area idata 007A 59D8 .word 23000 .area data 007C .blkb 2 .area idata 007C 5A3C .word 23100 .area data 007E .blkb 2 .area idata 007E 5AA0 .word 23200 .area data 0080 .blkb 2 .area idata 0080 5B04 .word 23300 .area data 0082 .blkb 2 .area idata 0082 5B68 .word 23400 .area data 0084 .blkb 2 .area idata 0084 5BCC .word 23500 .area data 0086 .blkb 2 .area idata 0086 5C30 .word 23600 .area data 0088 .blkb 2 .area idata 0088 5C94 .word 23700 .area data 008A .blkb 2 .area idata 008A 5CF8 .word 23800 .area data 008C .blkb 2 .area idata 008C 5D5C .word 23900 .area data 008E .blkb 2 .area idata 008E 5DC0 .word 24000 .area data 0090 .blkb 2 .area idata 0090 5E24 .word 24100 .area data 0092 .blkb 2 .area idata 0092 5E88 .word 24200 .area data 0094 .blkb 2 .area idata 0094 5EEC .word 24300 .area data 0096 .blkb 2 .area idata 0096 5F50 .word 24400 .area data 0098 .blkb 2 .area idata 0098 5FB4 .word 24500 .area data 009A .blkb 2 .area idata 009A 6018 .word 24600 .area data 009C .blkb 2 .area idata 009C 607C .word 24700 .area data 009E .blkb 2 .area idata 009E 60E0 .word 24800 .area data 00A0 .blkb 2 .area idata 00A0 6144 .word 24900 .area data 00A2 .blkb 2 .area idata 00A2 61A8 .word 25000 .area data 00A4 .blkb 2 .area idata 00A4 620C .word 25100 .area data 00A6 .blkb 2 .area idata 00A6 6270 .word 25200 .area data 00A8 .blkb 2 .area idata 00A8 62D4 .word 25300 .area data 00AA .blkb 2 .area idata 00AA 6338 .word 25400 .area data 00AC .blkb 2 .area idata 00AC 639C .word 25500 .area data 00AE .blkb 2 .area idata 00AE 6400 .word 25600 .area data 00B0 .blkb 2 .area idata 00B0 6464 .word 25700 .area data 00B2 .blkb 2 .area idata 00B2 64C8 .word 25800 .area data 00B4 .blkb 2 .area idata 00B4 652C .word 25900 .area data 00B6 .blkb 2 .area idata 00B6 6590 .word 26000 .area data 00B8 .blkb 2 .area idata 00B8 65F4 .word 26100 .area data 00BA .blkb 2 .area idata 00BA 6658 .word 26200 .area data 00BC .blkb 2 .area idata 00BC 66BC .word 26300 .area data 00BE .blkb 2 .area idata 00BE 6720 .word 26400 .area data 00C0 .blkb 2 .area idata 00C0 6784 .word 26500 .area data 00C2 .blkb 2 .area idata 00C2 67E8 .word 26600 .area data 00C4 .blkb 2 .area idata 00C4 684C .word 26700 .area data 00C6 .blkb 2 .area idata 00C6 68B0 .word 26800 .area data 00C8 .blkb 2 .area idata 00C8 6914 .word 26900 .area data 00CA .blkb 2 .area idata 00CA 6978 .word 27000 .area data 00CC .blkb 2 .area idata 00CC 69DC .word 27100 .area data 00CE .blkb 2 .area idata 00CE 6A40 .word 27200 .area data 00D0 .blkb 2 .area idata 00D0 6AA4 .word 27300 .area data 00D2 .blkb 2 .area idata 00D2 6B08 .word 27400 .area data 00D4 .blkb 2 .area idata 00D4 6B6C .word 27500 .area data 00D6 .blkb 2 .area idata 00D6 6BD0 .word 27600 .area data 00D8 .blkb 2 .area idata 00D8 6C34 .word 27700 .area data 00DA .blkb 2 .area idata 00DA 6C98 .word 27800 .area data 00DC .blkb 2 .area idata 00DC 6CFC .word 27900 .area data 00DE .blkb 2 .area idata 00DE 6D60 .word 28000 .area data 00E0 .blkb 2 .area idata 00E0 6DC4 .word 28100 .area data 00E2 .blkb 2 .area idata 00E2 6E28 .word 28200 .area data 00E4 .blkb 2 .area idata 00E4 6E8C .word 28300 .area data 00E6 .blkb 2 .area idata 00E6 6EF0 .word 28400 .area data 00E8 .blkb 2 .area idata 00E8 6F54 .word 28500 .area data 00EA .blkb 2 .area idata 00EA 6FB8 .word 28600 .area data 00EC .blkb 2 .area idata 00EC 701C .word 28700 .area data 00EE .blkb 2 .area idata 00EE 7080 .word 28800 .area data 00F0 .blkb 2 .area idata 00F0 70E4 .word 28900 .area data 00F2 .blkb 2 .area idata 00F2 7148 .word 29000 .area data 00F4 .blkb 2 .area idata 00F4 71AC .word 29100 .area data 00F6 .blkb 2 .area idata 00F6 7210 .word 29200 .area data 00F8 .blkb 2 .area idata 00F8 7274 .word 29300 .area data 00FA .blkb 2 .area idata 00FA 72D8 .word 29400 .area data 00FC .blkb 2 .area idata 00FC 733C .word 29500 .area data 00FE .blkb 2 .area idata 00FE 73A0 .word 29600 .area data 0100 .blkb 2 .area idata 0100 7404 .word 29700 .area data 0102 .blkb 2 .area idata 0102 7468 .word 29800 .area data 0104 .blkb 2 .area idata 0104 74CC .word 29900 .area data 0106 .blkb 2 .area idata 0106 7530 .word 30000 .area data 0108 .blkb 2 .area idata 0108 7594 .word 30100 .area data 010A .blkb 2 .area idata 010A 75F8 .word 30200 .area data 010C .blkb 2 .area idata 010C 765C .word 30300 .area data 010E .blkb 2 .area idata 010E 76C0 .word 30400 .area data 0110 .blkb 2 .area idata 0110 7724 .word 30500 .area data 0112 .blkb 2 .area idata 0112 7788 .word 30600 .area data 0114 .blkb 2 .area idata 0114 77EC .word 30700 .area data 0116 .blkb 2 .area idata 0116 7850 .word 30800 .area data 0118 .blkb 2 .area idata 0118 78B4 .word 30900 .area data 011A .blkb 2 .area idata 011A 7918 .word 31000 .area data 011C .blkb 2 .area idata 011C 797C .word 31100 .area data 011E .blkb 2 .area idata 011E 79E0 .word 31200 .area data 0120 .blkb 2 .area idata 0120 7A44 .word 31300 .area data 0122 .blkb 2 .area idata 0122 7AA8 .word 31400 .area data 0124 .blkb 2 .area idata 0124 7B0C .word 31500 .area data 0126 .blkb 2 .area idata 0126 7B70 .word 31600 .area data 0128 .blkb 2 .area idata 0128 7BD4 .word 31700 .area data 012A .blkb 2 .area idata 012A 7C38 .word 31800 .area data 012C .blkb 2 .area idata 012C 7C9C .word 31900 .area data 012E .blkb 2 .area idata 012E 7D00 .word 32000 .area data 0130 .blkb 2 .area idata 0130 7D64 .word 32100 .area data 0132 .blkb 2 .area idata 0132 7DC8 .word 32200 .area data 0134 .blkb 2 .area idata 0134 7E2C .word 32300 .area data 0136 .blkb 2 .area idata 0136 7E90 .word 32400 .area data 0138 .blkb 2 .area idata 0138 7EF4 .word 32500 .area data 013A .blkb 2 .area idata 013A 7F58 .word 32600 .area data 013C .blkb 2 .area idata 013C 7FBC .word 32700 .area data 013E .blkb 2 .area idata 013E 8020 .word -32736 .area data 0140 .blkb 2 .area idata 0140 8084 .word -32636 .area data 0142 .blkb 2 .area idata 0142 80E8 .word -32536 .area data 0144 .blkb 2 .area idata 0144 814C .word -32436 .area data 0146 .blkb 2 .area idata 0146 81B0 .word -32336 .area data 0148 .blkb 2 .area idata 0148 8214 .word -32236 .area data 014A .blkb 2 .area idata 014A 8278 .word -32136 .area data 014C .blkb 2 .area idata 014C 82DC .word -32036 .area data 014E .blkb 2 .area idata 014E 8340 .word -31936 .area data 0150 .blkb 2 .area idata 0150 83A4 .word -31836 .area data 0152 .blkb 2 .area idata 0152 8408 .word -31736 .area data 0154 .blkb 2 .area idata 0154 846C .word -31636 .area data 0156 .blkb 2 .area idata 0156 84D0 .word -31536 .area data 0158 .blkb 2 .area idata 0158 8534 .word -31436 .area data 015A .blkb 2 .area idata 015A 8598 .word -31336 .area data 015C .blkb 2 .area idata 015C 85FC .word -31236 .area data 015E .blkb 2 .area idata 015E 8660 .word -31136 .area data 0160 .blkb 2 .area idata 0160 86C4 .word -31036 .area data 0162 .blkb 2 .area idata 0162 8728 .word -30936 .area data 0164 .blkb 2 .area idata 0164 878C .word -30836 .area data 0166 .blkb 2 .area idata 0166 87F0 .word -30736 .area data 0168 .blkb 2 .area idata 0168 8854 .word -30636 .area data 016A .blkb 2 .area idata 016A 88B8 .word -30536 .area data 016C .blkb 2 .area idata 016C 891C .word -30436 .area data 016E .blkb 2 .area idata 016E 8980 .word -30336 .area data 0170 .blkb 2 .area idata 0170 89E4 .word -30236 .area data 0172 .blkb 2 .area idata 0172 8A48 .word -30136 .area data 0174 .blkb 2 .area idata 0174 8AAC .word -30036 .area data 0176 .blkb 2 .area idata 0176 8B10 .word -29936 .area data 0178 .blkb 2 .area idata 0178 8B74 .word -29836 .area data 017A .blkb 2 .area idata 017A 8BD8 .word -29736 .area data 017C .blkb 2 .area idata 017C 8C3C .word -29636 .area data 017E .blkb 2 .area idata 017E 8CA0 .word -29536 .area data 0180 .blkb 2 .area idata 0180 8D04 .word -29436 .area data 0182 .blkb 2 .area idata 0182 8D68 .word -29336 .area data 0184 .blkb 2 .area idata 0184 8DCC .word -29236 .area data 0186 .blkb 2 .area idata 0186 8E30 .word -29136 .area data 0188 .blkb 2 .area idata 0188 8E94 .word -29036 .area data 018A .blkb 2 .area idata 018A 8EF8 .word -28936 .area data 018C .blkb 2 .area idata 018C 8F5C .word -28836 .area data 018E .blkb 2 .area idata 018E 8FC0 .word -28736 .area data 0190 .blkb 2 .area idata 0190 9024 .word -28636 .area data 0192 .blkb 2 .area idata 0192 9088 .word -28536 .area data 0194 .blkb 2 .area idata 0194 90EC .word -28436 .area data 0196 .blkb 2 .area idata 0196 9150 .word -28336 .area data 0198 .blkb 2 .area idata 0198 91B4 .word -28236 .area data 019A .blkb 2 .area idata 019A 9218 .word -28136 .area data 019C .blkb 2 .area idata 019C 927C .word -28036 .area data 019E .blkb 2 .area idata 019E 92E0 .word -27936 .area data 01A0 .blkb 2 .area idata 01A0 9344 .word -27836 .area data 01A2 .blkb 2 .area idata 01A2 93A8 .word -27736 .area data 01A4 .blkb 2 .area idata 01A4 940C .word -27636 .area data 01A6 .blkb 2 .area idata 01A6 9470 .word -27536 .area data 01A8 .blkb 2 .area idata 01A8 94D4 .word -27436 .area data 01AA .blkb 2 .area idata 01AA 9538 .word -27336 .area data 01AC .blkb 2 .area idata 01AC 959C .word -27236 .area data 01AE .blkb 2 .area idata 01AE 9600 .word -27136 .area data 01B0 .blkb 2 .area idata 01B0 9664 .word -27036 .area data 01B2 .blkb 2 .area idata 01B2 96C8 .word -26936 .area data 01B4 .blkb 2 .area idata 01B4 972C .word -26836 .area data 01B6 .blkb 2 .area idata 01B6 9790 .word -26736 .area data 01B8 .blkb 2 .area idata 01B8 97F4 .word -26636 .area data 01BA .blkb 2 .area idata 01BA 9858 .word -26536 .area data 01BC .blkb 2 .area idata 01BC 98BC .word -26436 .area data 01BE .blkb 2 .area idata 01BE 9920 .word -26336 .area data 01C0 .blkb 2 .area idata 01C0 9984 .word -26236 .area data 01C2 .blkb 2 .area idata 01C2 99E8 .word -26136 .area data 01C4 .blkb 2 .area idata 01C4 9A4C .word -26036 .area data 01C6 .blkb 2 .area idata 01C6 9AB0 .word -25936 .area data 01C8 .blkb 2 .area idata 01C8 9B14 .word -25836 .area data 01CA .blkb 2 .area idata 01CA 9B78 .word -25736 .area data 01CC .blkb 2 .area idata 01CC 9BDC .word -25636 .area data 01CE .blkb 2 .area idata 01CE 9C40 .word -25536 .area data 01D0 .blkb 2 .area idata 01D0 9CA4 .word -25436 .area data 01D2 .blkb 2 .area idata 01D2 9D08 .word -25336 .area data 01D4 .blkb 2 .area idata 01D4 9D6C .word -25236 .area data 01D6 .blkb 2 .area idata 01D6 9DD0 .word -25136 .area data 01D8 .blkb 2 .area idata 01D8 9E34 .word -25036 .area data 01DA _Message:: 01DA .blkb 1 .area idata 01DA 00 .byte 0 .area data 01DB _CommandPos:: 01DB .blkb 1 .area idata 01DB 00 .byte 0 .area data 01DC _handleCommandFlag:: 01DC .blkb 1 .area idata 01DC 00 .byte 0 .area data 01DD _handleSampleFlag:: 01DD .blkb 1 .area idata 01DD 00 .byte 0 .area data 01DE _cyl1_pstn:: 01DE .blkb 2 .area idata 01DE 0000 .word 0 .area data 01E0 _cyl2_pstn:: 01E0 .blkb 2 .area idata 01E0 0000 .word 0 .area data 01E2 _cyl3_pstn:: 01E2 .blkb 2 .area idata 01E2 0000 .word 0 .area data 01E4 _cyl4_pstn:: 01E4 .blkb 2 .area idata 01E4 0000 .word 0 .area data 01E6 _cyl5_pstn:: 01E6 .blkb 2 .area idata 01E6 0000 .word 0 .area data 01E8 _cyl6_pstn:: 01E8 .blkb 2 .area idata 01E8 0000 .word 0 .area data 01EA _cyl7_pstn:: 01EA .blkb 2 .area idata 01EA 0000 .word 0 .area data 01EC _cyl8_pstn:: 01EC .blkb 2 .area idata 01EC 0000 .word 0 .area data 01EE _cyl1_des:: 01EE .blkb 2 .area idata 01EE 0064 .word 100 .area data 01F0 _cyl2_des:: 01F0 .blkb 2 .area idata 01F0 0064 .word 100 .area data 01F2 _cyl3_des:: 01F2 .blkb 2 .area idata 01F2 0064 .word 100 .area data 01F4 _cyl4_des:: 01F4 .blkb 2 .area idata 01F4 0064 .word 100 .area data 01F6 _cyl5_des:: 01F6 .blkb 2 .area idata 01F6 0064 .word 100 .area data 01F8 _cyl6_des:: 01F8 .blkb 2 .area idata 01F8 0064 .word 100 .area data 01FA _cyl7_des:: 01FA .blkb 2 .area idata 01FA 0064 .word 100 .area data 01FC _cyl8_des:: 01FC .blkb 2 .area idata 01FC 0064 .word 100 .area data 01FE _error1:: 01FE .blkb 2 .area idata 01FE 0000 .word 0 .area data 0200 .blkb 2 .area idata 0200 0000 .word 0 .area data 0202 .blkb 2 .area idata 0202 0000 .word 0 .area data 0204 _error2:: 0204 .blkb 2 .area idata 0204 0000 .word 0 .area data 0206 .blkb 2 .area idata 0206 0000 .word 0 .area data 0208 .blkb 2 .area idata 0208 0000 .word 0 .area data 020A _error3:: 020A .blkb 2 .area idata 020A 0000 .word 0 .area data 020C .blkb 2 .area idata 020C 0000 .word 0 .area data 020E .blkb 2 .area idata 020E 0000 .word 0 .area data 0210 _error4:: 0210 .blkb 2 .area idata 0210 0000 .word 0 .area data 0212 .blkb 2 .area idata 0212 0000 .word 0 .area data 0214 .blkb 2 .area idata 0214 0000 .word 0 .area data 0216 _error5:: 0216 .blkb 2 .area idata 0216 0000 .word 0 .area data 0218 .blkb 2 .area idata 0218 0000 .word 0 .area data 021A .blkb 2 .area idata 021A 0000 .word 0 .area data 021C _error6:: 021C .blkb 2 .area idata 021C 0000 .word 0 .area data 021E .blkb 2 .area idata 021E 0000 .word 0 .area data 0220 .blkb 2 .area idata 0220 0000 .word 0 .area data 0222 _error7:: 0222 .blkb 2 .area idata 0222 0000 .word 0 .area data 0224 .blkb 2 .area idata 0224 0000 .word 0 .area data 0226 .blkb 2 .area idata 0226 0000 .word 0 .area data 0228 _error8:: 0228 .blkb 2 .area idata 0228 0000 .word 0 .area data 022A .blkb 2 .area idata 022A 0000 .word 0 .area data 022C .blkb 2 .area idata 022C 0000 .word 0 .area data 022E _K1:: 022E .blkb 2 .area idata 022E 0003 .word 3 .area data 0230 .blkb 2 .area idata 0230 0000 .word 0 .area data 0232 .blkb 2 .area idata 0232 0000 .word 0 .area data 0234 _K2:: 0234 .blkb 2 .area idata 0234 0003 .word 3 .area data 0236 .blkb 2 .area idata 0236 0000 .word 0 .area data 0238 .blkb 2 .area idata 0238 0000 .word 0 .area data 023A _K3:: 023A .blkb 2 .area idata 023A 0003 .word 3 .area data 023C .blkb 2 .area idata 023C 0000 .word 0 .area data 023E .blkb 2 .area idata 023E 0000 .word 0 .area data 0240 _K4:: 0240 .blkb 2 .area idata 0240 0003 .word 3 .area data 0242 .blkb 2 .area idata 0242 0000 .word 0 .area data 0244 .blkb 2 .area idata 0244 0000 .word 0 .area data 0246 _K5:: 0246 .blkb 2 .area idata 0246 0003 .word 3 .area data 0248 .blkb 2 .area idata 0248 0000 .word 0 .area data 024A .blkb 2 .area idata 024A 0000 .word 0 .area data 024C _K6:: 024C .blkb 2 .area idata 024C 0003 .word 3 .area data 024E .blkb 2 .area idata 024E 0000 .word 0 .area data 0250 .blkb 2 .area idata 0250 0000 .word 0 .area data 0252 _K7:: 0252 .blkb 2 .area idata 0252 0003 .word 3 .area data 0254 .blkb 2 .area idata 0254 0000 .word 0 .area data 0256 .blkb 2 .area idata 0256 0000 .word 0 .area data 0258 _K8:: 0258 .blkb 2 .area idata 0258 0003 .word 3 .area data 025A .blkb 2 .area idata 025A 0000 .word 0 .area data 025C .blkb 2 .area idata 025C 0000 .word 0 .area data 025E _motor1:: 025E .blkb 2 .area idata 025E 0000 .word 0 .area data 0260 _motor2:: 0260 .blkb 2 .area idata 0260 0000 .word 0 .area data 0262 _motor3:: 0262 .blkb 2 .area idata 0262 0000 .word 0 .area data 0264 _motor4:: 0264 .blkb 2 .area idata 0264 0000 .word 0 .area data 0266 _motor5:: 0266 .blkb 2 .area idata 0266 0000 .word 0 .area data 0268 _motor6:: 0268 .blkb 2 .area idata 0268 0000 .word 0 .area data 026A _motor7:: 026A .blkb 2 .area idata 026A 0000 .word 0 .area data 026C _motor8:: 026C .blkb 2 .area idata 026C 0000 .word 0 .area data 026E _calc_1ot:: 026E .blkb 1 .area idata 026E 00 .byte 0 .area data 026F _calc_1vl:: 026F .blkb 2 .area idata 026F 0000 .word 0 .area data 0271 _calc_2ot:: 0271 .blkb 1 .area idata 0271 00 .byte 0 .area data 0272 _calc_2vl:: 0272 .blkb 2 .area idata 0272 0000 .word 0 .area data 0274 _calc_3ot:: 0274 .blkb 1 .area idata 0274 00 .byte 0 .area data 0275 _calc_3vl:: 0275 .blkb 2 .area idata 0275 0000 .word 0 .area data 0277 _new_motor_num:: 0277 .blkb 1 .area idata 0277 00 .byte 0 .area data 0278 _new_motor_spd:: 0278 .blkb 2 .area idata 0278 0000 .word 0 .area data 027A _comp_motor_spd:: 027A .blkb 2 .area idata 027A 0000 .word 0 .area data 027C _first_motor:: 027C .blkb 1 .area idata 027C 00 .byte 0 .area data 027D _same_motor:: 027D .blkb 1 .area idata 027D 00 .byte 0 .area data 027E _oc5_reg:: 027E .blkb 2 .area idata 027E 0000 .word 0 .area data 0280 _oc5_1ot:: 0280 .blkb 1 .area idata 0280 00 .byte 0 .area data 0281 _oc5_1vl:: 0281 .blkb 2 .area idata 0281 0000 .word 0 .area data 0283 _oc5_2ot:: 0283 .blkb 1 .area idata 0283 00 .byte 0 .area data 0284 _oc5_2vl:: 0284 .blkb 2 .area idata 0284 0000 .word 0 .area data 0286 _oc5_3ot:: 0286 .blkb 1 .area idata 0286 00 .byte 0 .area data 0287 _oc5_3vl:: 0287 .blkb 2 .area idata 0287 0000 .word 0 .area data 0289 _oc4_reg:: 0289 .blkb 2 .area idata 0289 0000 .word 0 .area data 028B _oc4_1ot:: 028B .blkb 1 .area idata 028B 00 .byte 0 .area data 028C _oc4_1vl:: 028C .blkb 2 .area idata 028C 0000 .word 0 .area data 028E _oc4_2ot:: 028E .blkb 1 .area idata 028E 00 .byte 0 .area data 028F _oc4_2vl:: 028F .blkb 2 .area idata 028F 0000 .word 0 .area data 0291 _oc4_3ot:: 0291 .blkb 1 .area idata 0291 00 .byte 0 .area data 0292 _oc4_3vl:: 0292 .blkb 2 .area idata 0292 0000 .word 0 .area data 0294 _oc3_reg:: 0294 .blkb 2 .area idata 0294 0000 .word 0 .area data 0296 _oc3_1ot:: 0296 .blkb 1 .area idata 0296 00 .byte 0 .area data 0297 _oc3_1vl:: 0297 .blkb 2 .area idata 0297 0000 .word 0 .area data 0299 _oc3_2ot:: 0299 .blkb 1 .area idata 0299 00 .byte 0 .area data 029A _oc3_2vl:: 029A .blkb 2 .area idata 029A 0000 .word 0 .area data 029C _oc3_3ot:: 029C .blkb 1 .area idata 029C 00 .byte 0 .area data 029D _oc3_3vl:: 029D .blkb 2 .area idata 029D 0000 .word 0 .area data 029F _oc2_reg:: 029F .blkb 2 .area idata 029F 0000 .word 0 .area data 02A1 _oc2_1ot:: 02A1 .blkb 1 .area idata 02A1 00 .byte 0 .area data 02A2 _oc2_1vl:: 02A2 .blkb 2 .area idata 02A2 0000 .word 0 .area data 02A4 _oc2_2ot:: 02A4 .blkb 1 .area idata 02A4 00 .byte 0 .area data 02A5 _oc2_2vl:: 02A5 .blkb 2 .area idata 02A5 0000 .word 0 .area data 02A7 _oc2_3ot:: 02A7 .blkb 1 .area idata 02A7 00 .byte 0 .area data 02A8 _oc2_3vl:: 02A8 .blkb 2 .area idata 02A8 0000 .word 0 .area data 02AA _x_preserve:: 02AA .blkb 2 .area idata 02AA 0000 .word 0 .area data 02AC _mot_spd:: 02AC .blkb 1 .area idata 02AC 00 .byte 0 .area data 02AD _pwm_port:: 02AD .blkb 2 .area idata 02AD 5000 .word 20480 .area data 02AF _wave_period:: 02AF .blkb 2 .area idata 02AF EA60 .word -5536 .area data 02B1 _RTIcount:: 02B1 .blkb 1 .area idata 02B1 00 .byte 0 .area text ; IX -> 0,x 0000 _init_SCI:: 0000 .dbfile demoday.c 0000 .dbfunc init_SCI 0000 .dbline 171 ; /*----------------------------------------------------* ; *Program : demoday.c * ; *Version : HC!!-1.0 * ; *Programmer : Scott Kanowitz * ; *Contracter : Machine Intelligence Labroatory * ; * University of Florida * ; *Project : Pneuman * ; *Date : 3/19/2000 * ; *----------------------------------------------------* ; * Description : A program to run on the HC11 to * ; * control 8 cylinders via a PID * ; * controller and communicate with * ; * the host machine via serial line * ; *----------------------------------------------------*/ ; ; ; //#include ; #include ; #include ; #include ; #include ; ; /* Set up interrupt vectors (also at end) */ ; extern void _start(void); /* entry point in crt??.s */ ; ; ; #pragma interrupt_handler TOC2_isr ; #pragma interrupt_handler TOC3_isr ; #pragma interrupt_handler TOC4_isr ; #pragma interrupt_handler TOC5_isr ; #pragma interrupt_handler SCI_isr ; #pragma interrupt_handler RTI_isr ; ; ; ; #define DUMMY_ENTRY (void (*)(void))0xFFFF /*used as blank space in inpt vectors*/ ; #define COMMAND_LENGTH 7 //(start char) + (5 char command(ie C1150)) + (end char) ; #define START_CHAR 0x24 //corresponds to $ ; #define END_CHAR 0x23 //corresponds to # ; ; #define P 0 ; #define I 1 ; #define D 2 ; ; #define TOLERANCE 4 ; ; #define DIR_PORT *(unsigned char *)(0x4000) ; /*--------------------------------------------------* ; *Global variables * ; * * ; *--------------------------------------------------*/ ; //char cylinderNumber = 0; ; ; char Message = 0; ; char CommandPos = 0; ; char handleCommandFlag = 0; ; char Command[2* COMMAND_LENGTH]; ; ; char handleSampleFlag = 0; ; ; int cyl1_pstn = 0; ; int cyl2_pstn = 0; ; int cyl3_pstn = 0; ; int cyl4_pstn = 0; ; int cyl5_pstn = 0; ; int cyl6_pstn = 0; ; int cyl7_pstn = 0; ; int cyl8_pstn = 0; ; ; int cyl1_des = 100; ; int cyl2_des = 100; ; int cyl3_des = 100; ; int cyl4_des = 100; ; int cyl5_des = 100; ; int cyl6_des = 100; ; int cyl7_des = 100; ; int cyl8_des = 100; ; ; int error1[3] = {0, 0, 0}; ; int error2[3] = {0, 0, 0}; ; int error3[3] = {0, 0, 0}; ; int error4[3] = {0, 0, 0}; ; int error5[3] = {0, 0, 0}; ; int error6[3] = {0, 0, 0}; ; int error7[3] = {0, 0, 0}; ; int error8[3] = {0, 0, 0}; ; ; int K1[3] = {3, 0, 0}; ; int K2[3] = {3, 0, 0}; ; int K3[3] = {3, 0, 0}; ; int K4[3] = {3, 0, 0}; ; int K5[3] = {3, 0, 0}; ; int K6[3] = {3, 0, 0}; ; int K7[3] = {3, 0, 0}; ; int K8[3] = {3, 0, 0}; ; ; ; int motor1 =0; ; int motor2 =0; ; int motor3 =0; ; int motor4 =0; ; int motor5 =0; ; int motor6 =0; ; int motor7 =0; ; int motor8 =0; ; ; char calc_1ot =0; ; int calc_1vl =0; ; char calc_2ot =0; ; int calc_2vl =0; ; char calc_3ot =0; ; int calc_3vl =0; ; ; char new_motor_num = 0; ; int new_motor_spd = 0; ; int comp_motor_spd = 0; ; char first_motor = 0; ; char same_motor = 0; ; ; char *oc5_reg = 0; ; char oc5_1ot = 0; ; int oc5_1vl = 0; ; char oc5_2ot = 0; ; int oc5_2vl = 0; ; char oc5_3ot = 0; ; int oc5_3vl = 0; ; ; char *oc4_reg = 0; ; char oc4_1ot = 0; ; int oc4_1vl = 0; ; char oc4_2ot = 0; ; int oc4_2vl = 0; ; char oc4_3ot = 0; ; int oc4_3vl = 0; ; ; char *oc3_reg = 0; ; char oc3_1ot = 0; ; int oc3_1vl = 0; ; char oc3_2ot = 0; ; int oc3_2vl = 0; ; char oc3_3ot = 0; ; int oc3_3vl = 0; ; ; char *oc2_reg = 0; ; char oc2_1ot = 0; ; int oc2_1vl = 0; ; char oc2_2ot = 0; ; int oc2_2vl = 0; ; char oc2_3ot = 0; ; int oc2_3vl = 0; ; ; int x_preserve = 0; ; ; char mot_spd = 0; ; ; int pwm_port = 0x5000; ; ; int wave_period = 0xEA60; ; ; char RTIcount =0; ; ; ; /************************************************************ ; ; INITIALIZATION FUNCTIONS ; ; *************************************************************/ ; ; /* Set SCI for 9600, 8-n-1, TE, RE, and rx interrupt enabled, tx disabled */ ; void init_SCI(){ ; BAUD |= 0X30; 0000 18CE102B ldy #0x102b 0004 181C0030 bset 0,y,#48 0008 .dbline 172 ; SCCR1 = 0X00; 0008 7F102C clr 0x102c 000B .dbline 173 ; SCCR2 |= 0X2C; 000B 18CE102D ldy #0x102d 000F 181C002C bset 0,y,#44 0013 .dbline 174 ; SCCR2 &= 0X7f; 0013 18CE102D ldy #0x102d 0017 181D0080 bclr 0,y,#0x80 001B .dbline 175 ; } 001B L2: 001B 39 rts ; IX -> 0,x 001C _init_pulse:: 001C 0F sei 001D .dbfunc init_pulse 001D .dbline 182 ; ; ; /* Set up oc2-5 to interrupt and dissconnect from pins*/ ; void init_pulse() ; { ; INTR_OFF(); ; CLEAR_BIT(TCTL1, 0xFF); //Disconnect all OCs from pins 001D 18CE1020 ldy #0x1020 0021 181D00FF bclr 0,y,#0xff 0025 .dbline 183 ; TOC2 = 3000; 0025 CC0BB8 ldd #3000 0028 FD1018 std 0x1018 002B .dbline 184 ; TOC3 = TOC2+3000; //offset all the initial interrupt times ; vol 002B FC1018 ldd 0x1018 002E C30BB8 addd #3000 0031 FD101A std 0x101a 0034 .dbline 185 ; TOC4 = TOC3+3000; ; vol 0034 FC101A ldd 0x101a 0037 C30BB8 addd #3000 003A FD101C std 0x101c 003D .dbline 186 ; TOC5 = TOC4+3000; ; vol 003D FC101C ldd 0x101c 0040 C30BB8 addd #3000 0043 FD101E std 0x101e 0046 .dbline 187 ; SET_BIT(TMSK1, 0x78); //enable oc2-5 0046 18CE1022 ldy #0x1022 004A 181C0078 bset 0,y,#120 004E .dbline 188 ; CLEAR_BIT(PACTL, 0x04); //Enable oc5 004E 18CE1026 ldy #0x1026 0052 181D0004 bclr 0,y,#0x4 0056 0E cli 0057 .dbline 190 ; INTR_ON(); //turn on interrupts ; } 0057 L3: 0057 39 rts ; IX -> 0,x 0058 _init_RTI:: 0058 .dbfunc init_RTI 0058 .dbline 195 ; ; ; /* Initialize RTI to interrupt every 32.768ms */ ; void init_RTI() { ; PACTL |= 0X03; 0058 18CE1026 ldy #0x1026 005C 181C0003 bset 0,y,#3 0060 .dbline 196 ; TMSK2 |= 0X40; 0060 18CE1024 ldy #0x1024 0064 181C0040 bset 0,y,#64 0068 .dbline 197 ; } 0068 L4: 0068 39 rts ; IX -> 0,x ; mot_num -> 9,x ; new_duty -> 4,x 0069 _multvlve:: 0069 37 pshb 006A 36 psha 006B 3C pshx 006C 3C pshx 006D 30 tsx 006E EF00 stx 0,x 0070 .dbfunc multvlve 0070 .dbline 208 ; ; ; /************************************************************ ; ; MULTVLVE function to set up duty_cycles ; ; *************************************************************/ ; ; void multvlve(int new_duty, char mot_num) ; { ; new_motor_spd = new_duty; 0070 EC04 ldd 4,x 0072 FD0278 std _new_motor_spd 0075 .dbline 209 ; new_motor_num = mot_num; 0075 E609 ldab 9,x 0077 F70277 stab _new_motor_num 007A FF02AA stx _x_preserve 007D F60277 ldab _new_motor_num 0080 C101 cmpb #$01 0082 2614 bne is_motor_2 0084 FE0278 ldx _new_motor_spd 0087 FF025E stx _motor1 008A FE0260 ldx _motor2 008D FF027A stx _comp_motor_spd 0090 C601 ldab #$01 0092 F7027C stab _first_motor 0095 7E013A jmp calc_motor_times 0098 is_motor_2: 0098 C102 cmpb #$02 009A 2612 bne is_motor_3 009C FE0278 ldx _new_motor_spd 009F FF0260 stx _motor2 00A2 FE025E ldx _motor1 00A5 FF027A stx _comp_motor_spd 00A8 7F027C clr _first_motor 00AB 7E013A jmp calc_motor_times 00AE is_motor_3: 00AE C103 cmpb #$03 00B0 2614 bne is_motor_4 00B2 FE0278 ldx _new_motor_spd 00B5 FF0262 stx _motor3 00B8 FE0264 ldx _motor4 00BB FF027A stx _comp_motor_spd 00BE C601 ldab #$01 00C0 F7027C stab _first_motor 00C3 7E013A jmp calc_motor_times 00C6 is_motor_4: 00C6 C104 cmpb #$04 00C8 2612 bne is_motor_5 00CA FE0278 ldx _new_motor_spd 00CD FF0264 stx _motor4 00D0 FE0262 ldx _motor3 00D3 FF027A stx _comp_motor_spd 00D6 7F027C clr _first_motor 00D9 7E013A jmp calc_motor_times 00DC is_motor_5: 00DC C105 cmpb #$05 00DE 2614 bne is_motor_6 00E0 FE0278 ldx _new_motor_spd 00E3 FF0266 stx _motor5 00E6 FE0268 ldx _motor6 00E9 FF027A stx _comp_motor_spd 00EC C601 ldab #$01 00EE F7027C stab _first_motor 00F1 7E013A jmp calc_motor_times 00F4 is_motor_6: 00F4 C106 cmpb #$06 00F6 2612 bne is_motor_7 00F8 FE0278 ldx _new_motor_spd 00FB FF0268 stx _motor6 00FE FE0266 ldx _motor5 0101 FF027A stx _comp_motor_spd 0104 7F027C clr _first_motor 0107 7E013A jmp calc_motor_times 010A is_motor_7: 010A C107 cmpb #$07 010C 2614 bne is_motor_8 010E FE0278 ldx _new_motor_spd 0111 FF026A stx _motor7 0114 FE026C ldx _motor8 0117 FF027A stx _comp_motor_spd 011A C601 ldab #$01 011C F7027C stab _first_motor 011F 7E013A jmp calc_motor_times 0122 is_motor_8: 0122 C108 cmpb #$08 0124 2611 bne end_routine 0126 FE0278 ldx _new_motor_spd 0129 FF026C stx _motor8 012C FE026A ldx _motor7 012F FF027A stx _comp_motor_spd 0132 7F027C clr _first_motor 0135 2003 bra calc_motor_times 0137 end_routine: 0137 7E045D jmp multvlve_done 013A calc_motor_times: 013A 7F027D clr _same_motor 013D FE0278 ldx _new_motor_spd 0140 BC027A cpx _comp_motor_spd 0143 2605 bne calc_motor_nequ 0145 8601 ldaa #$01 0147 B7027D staa _same_motor 014A calc_motor_nequ: 014A BC02AF cpx _wave_period 014D 2603 bne calc_mot_con1 014F 7E0312 jmp clc_mot_40 0152 calc_mot_con1: 0152 8C0000 cpx #$0000 0155 2603 bne calc_mot_con2 0157 7E027A jmp clc_mot_00 015A calc_mot_con2: 015A B6027D ldaa _same_motor 015D 266C bne calc_motor_same 015F BC027A cpx _comp_motor_spd 0162 2303 bls clc_mot_les 0164 7E01F3 jmp clc_mot_gt 0167 clc_mot_les: 0167 FF026F stx _calc_1vl 016A FC027A ldd _comp_motor_spd 016D B30278 subd _new_motor_spd 0170 FD0272 std _calc_2vl 0173 FC02AF ldd _wave_period 0176 1AB3027A cpd _comp_motor_spd 017A 271B beq clc_mot_lesc 017C B30272 subd _calc_2vl 017F B3026F subd _calc_1vl 0182 FD0275 std _calc_3vl 0185 8603 ldaa #0b00000011 0187 B7026E staa _calc_1ot 018A 8602 ldaa #0b00000010 018C B70271 staa _calc_2ot 018F 8600 ldaa #0b00000000 0191 B70274 staa _calc_3ot 0194 7E0397 jmp calc_mot_fin 0197 clc_mot_lesc: 0197 B30278 subd _new_motor_spd 019A 8303E8 subd #1000 019D FD0275 std _calc_3vl 01A0 CC03E8 ldd #1000 01A3 FD0272 std _calc_2vl 01A6 B6027C ldaa _first_motor 01A9 2610 bne is_mot_lessc 01AB 8603 ldaa #0b00000011 01AD B7026E staa _calc_1ot 01B0 8601 ldaa #0b00000001 01B2 B70274 staa _calc_3ot 01B5 B70271 staa _calc_2ot 01B8 7E0397 jmp calc_mot_fin 01BB is_mot_lessc: 01BB 8603 ldaa #0b00000011 01BD B7026E staa _calc_1ot 01C0 8602 ldaa #0b00000010 01C2 B70274 staa _calc_3ot 01C5 B70271 staa _calc_2ot 01C8 7E0397 jmp calc_mot_fin 01CB calc_motor_same: 01CB FC0278 ldd _new_motor_spd 01CE FD026F std _calc_1vl 01D1 CC03E8 ldd #1000 01D4 FD0272 std _calc_2vl 01D7 FC02AF ldd _wave_period 01DA B30272 subd _calc_2vl 01DD B3026F subd _calc_1vl 01E0 FD0275 std _calc_3vl 01E3 8603 ldaa #0b00000011 01E5 B7026E staa _calc_1ot 01E8 8600 ldaa #$00000000 01EA B70274 staa _calc_3ot 01ED B70271 staa _calc_2ot 01F0 7E0397 jmp calc_mot_fin 01F3 clc_mot_gt: 01F3 FC027A ldd _comp_motor_spd 01F6 1A830000 cpd #$00 01FA 2741 beq clc_mot_gtr0 01FC FD026F std _calc_1vl 01FF FC0278 ldd _new_motor_spd 0202 B3027A subd _comp_motor_spd 0205 FD0272 std _calc_2vl 0208 FC02AF ldd _wave_period 020B B30272 subd _calc_2vl 020E B3026F subd _calc_1vl 0211 FD0275 std _calc_3vl 0214 B6027C ldaa _first_motor 0217 2612 bne is_mot_gtrc 0219 8603 ldaa #0b00000011 021B B7026E staa _calc_1ot 021E 8602 ldaa #0b00000010 0220 B70271 staa _calc_2ot 0223 8600 ldaa #0b00000000 0225 B70274 staa _calc_3ot 0228 7E0397 jmp calc_mot_fin 022B is_mot_gtrc: 022B 8603 ldaa #0b00000011 022D B7026E staa _calc_1ot 0230 8601 ldaa #0b00000001 0232 B70271 staa _calc_2ot 0235 8600 ldaa #0b00000000 0237 B70274 staa _calc_3ot 023A 7E0397 jmp calc_mot_fin 023D clc_mot_gtr0: 023D FC0278 ldd _new_motor_spd 0240 FD026F std _calc_1vl 0243 CC03E8 ldd #1000 0246 FD0272 std _calc_2vl 0249 FC02AF ldd _wave_period 024C B30272 subd _calc_2vl 024F B3026F subd _calc_1vl 0252 FD0275 std _calc_3vl 0255 B6027C ldaa _first_motor 0258 2610 bne is_mot_gtrcc 025A 8602 ldaa #0b00000010 025C B7026E staa _calc_1ot 025F 8600 ldaa #0b00000000 0261 B70274 staa _calc_3ot 0264 B70271 staa _calc_2ot 0267 7E0397 jmp calc_mot_fin 026A is_mot_gtrcc: 026A 8601 ldaa #0b00000001 026C B7026E staa _calc_1ot 026F 8600 ldaa #0b00000000 0271 B70274 staa _calc_3ot 0274 B70271 staa _calc_2ot 0277 7E0397 jmp calc_mot_fin 027A clc_mot_00: 027A B6027D ldaa _same_motor 027D 2643 bne both_zero 027F FC027A ldd _comp_motor_spd 0282 1AB302AF cpd _wave_period 0286 2760 beq clc_mot_0040 0288 FD026F std _calc_1vl 028B CC03E8 ldd #1000 028E FD0272 std _calc_2vl 0291 FC02AF ldd _wave_period 0294 B30272 subd _calc_2vl 0297 B3026F subd _calc_1vl 029A FD0275 std _calc_3vl 029D B6027C ldaa _first_motor 02A0 2610 bne is_mot_00 02A2 8601 ldaa #0b00000001 02A4 B7026E staa _calc_1ot 02A7 8600 ldaa #0b00000000 02A9 B70274 staa _calc_3ot 02AC B70271 staa _calc_2ot 02AF 7E0397 jmp calc_mot_fin 02B2 is_mot_00: 02B2 8602 ldaa #0b00000010 02B4 B7026E staa _calc_1ot 02B7 8600 ldaa #0b00000000 02B9 B70274 staa _calc_3ot 02BC B70271 staa _calc_2ot 02BF 7E0397 jmp calc_mot_fin 02C2 both_zero: 02C2 8600 ldaa #0b00000000 02C4 B7026E staa _calc_1ot 02C7 B70271 staa _calc_2ot 02CA B70274 staa _calc_3ot 02CD CC03E8 ldd #1000 02D0 FD026F std _calc_1vl 02D3 CC03E8 ldd #1000 02D6 FD0272 std _calc_2vl 02D9 FC02AF ldd _wave_period 02DC B30272 subd _calc_2vl 02DF B3026F subd _calc_1vl 02E2 FD0275 std _calc_3vl 02E5 7E0397 jmp calc_mot_fin 02E8 clc_mot_0040: 02E8 CC03E8 ldd #1000 02EB FD026F std _calc_1vl 02EE FD0272 std _calc_2vl 02F1 FC02AF ldd _wave_period 02F4 B3026F subd _calc_1vl 02F7 B30272 subd _calc_2vl 02FA FD0275 std _calc_3vl 02FD 8602 ldaa #0b00000010 02FF F6027C ldab _first_motor 0302 2602 bne is_mot_0040 0304 8601 ldaa #0b00000001 0306 is_mot_0040: 0306 B7026E staa _calc_1ot 0309 B70271 staa _calc_2ot 030C B70274 staa _calc_3ot 030F 7E0397 jmp calc_mot_fin 0312 clc_mot_40: 0312 B6027D ldaa _same_motor 0315 265A bne both_40000 0317 FC027A ldd _comp_motor_spd 031A 1A830000 cpd #$0000 031E 2603 bne clc_mot_40c 0320 CC03E8 ldd #1000 0323 clc_mot_40c: 0323 FD026F std _calc_1vl 0326 CC03E8 ldd #1000 0329 FD0272 std _calc_2vl 032C FC02AF ldd _wave_period 032F B30272 subd _calc_2vl 0332 B3026F subd _calc_1vl 0335 FD0275 std _calc_3vl 0338 B6027C ldaa _first_motor 033B 261A bne is_mot_40 033D 8603 ldaa #0b00000011 033F FE027A ldx _comp_motor_spd 0342 8C0000 cpx #$0000 0345 2602 bne clc_mot_40cc 0347 8602 ldaa #0b00000010 0349 clc_mot_40cc: 0349 B7026E staa _calc_1ot 034C 8602 ldaa #0b00000010 034E B70274 staa _calc_3ot 0351 B70271 staa _calc_2ot 0354 7E0397 jmp calc_mot_fin 0357 is_mot_40: 0357 8603 ldaa #0b00000011 0359 FE027A ldx _comp_motor_spd 035C 8C0000 cpx #$0000 035F 2602 bne is_mot_40c 0361 8601 ldaa #0b00000001 0363 is_mot_40c: 0363 B7026E staa _calc_1ot 0366 8601 ldaa #0b00000001 0368 B70274 staa _calc_3ot 036B B70271 staa _calc_2ot 036E 7E0397 jmp calc_mot_fin 0371 both_40000: 0371 CC03E8 ldd #1000 0374 FD026F std _calc_1vl 0377 CC03E8 ldd #1000 037A FD0272 std _calc_2vl 037D FC02AF ldd _wave_period 0380 B30272 subd _calc_2vl 0383 B3026F subd _calc_1vl 0386 FD0275 std _calc_3vl 0389 8603 ldaa #0b00000011 038B B7026E staa _calc_1ot 038E B70271 staa _calc_2ot 0391 B70274 staa _calc_3ot 0394 7E0397 jmp calc_mot_fin 0397 calc_mot_fin: 0397 18CE02A1 ldy #_oc2_1ot 039B B60277 ldaa _new_motor_num 039E 8101 cmpa #$01 03A0 2716 beq jmp_calc_motor_done 03A2 8102 cmpa #$02 03A4 2712 beq jmp_calc_motor_done 03A6 8103 cmpa #$03 03A8 2761 beq calc_motor_34 03AA 8104 cmpa #$04 03AC 275D beq calc_motor_34 03AE 8105 cmpa #$05 03B0 2734 beq calc_motor_56 03B2 8106 cmpa #$06 03B4 2730 beq calc_motor_56 03B6 2003 bra calc_motor_78 03B8 jmp_calc_motor_done: 03B8 7E0427 jmp calc_motor_done 03BB calc_motor_78: 03BB B6026E ldaa _calc_1ot 03BE 48 lsla 03BF 48 lsla 03C0 48 lsla 03C1 48 lsla 03C2 48 lsla 03C3 48 lsla 03C4 B7026E staa _calc_1ot 03C7 B60271 ldaa _calc_2ot 03CA 48 lsla 03CB 48 lsla 03CC 48 lsla 03CD 48 lsla 03CE 48 lsla 03CF 48 lsla 03D0 B70271 staa _calc_2ot 03D3 B60274 ldaa _calc_3ot 03D6 48 lsla 03D7 48 lsla 03D8 48 lsla 03D9 48 lsla 03DA 48 lsla 03DB 48 lsla 03DC B70274 staa _calc_3ot 03DF 18CE0280 ldy #_oc5_1ot 03E3 7E0427 jmp calc_motor_done 03E6 calc_motor_56: 03E6 B6026E ldaa _calc_1ot 03E9 48 lsla 03EA 48 lsla 03EB 48 lsla 03EC 48 lsla 03ED B7026E staa _calc_1ot 03F0 B60271 ldaa _calc_2ot 03F3 48 lsla 03F4 48 lsla 03F5 48 lsla 03F6 48 lsla 03F7 B70271 staa _calc_2ot 03FA B60274 ldaa _calc_3ot 03FD 48 lsla 03FE 48 lsla 03FF 48 lsla 0400 48 lsla 0401 B70274 staa _calc_3ot 0404 18CE028B ldy #_oc4_1ot 0408 7E0427 jmp calc_motor_done 040B calc_motor_34: 040B B6026E ldaa _calc_1ot 040E 48 lsla 040F 48 lsla 0410 B7026E staa _calc_1ot 0413 B60271 ldaa _calc_2ot 0416 48 lsla 0417 48 lsla 0418 B70271 staa _calc_2ot 041B B60274 ldaa _calc_3ot 041E 48 lsla 041F 48 lsla 0420 B70274 staa _calc_3ot 0423 18CE0296 ldy #_oc3_1ot 0427 calc_motor_done: 0427 CE026E ldx #_calc_1ot 042A A600 ldaa 0,x 042C 18A700 staa 0,y 042F 08 inx 0430 1808 iny 0432 EC00 ldd 0,x 0434 18ED00 std 0,y 0437 08 inx 0438 08 inx 0439 1808 iny 043B 1808 iny 043D A600 ldaa 0,x 043F 18A700 staa 0,y 0442 08 inx 0443 1808 iny 0445 EC00 ldd 0,x 0447 18ED00 std 0,y 044A 08 inx 044B 08 inx 044C 1808 iny 044E 1808 iny 0450 A600 ldaa 0,x 0452 18A700 staa 0,y 0455 08 inx 0456 1808 iny 0458 EC00 ldd 0,x 045A 18ED00 std 0,y 045D multvlve_done: 045D FE02AA ldx _x_preserve 0460 .dbline 644 ; ; ; asm("stx _x_preserve\n" ; "ldab _new_motor_num\n" ; "cmpb #$01\n" ; "bne is_motor_2\n" ; "ldx _new_motor_spd\n" ; "stx _motor1\n" ; "ldx _motor2\n" ; "stx _comp_motor_spd\n" ; "ldab #$01\n" ; "stab _first_motor\n" ; "jmp calc_motor_times\n" ; "is_motor_2:\n" ; "cmpb #$02\n" ; "bne is_motor_3\n" ; "ldx _new_motor_spd\n" ; "stx _motor2\n" ; "ldx _motor1\n" ; "stx _comp_motor_spd\n" ; "clr _first_motor\n" ; "jmp calc_motor_times"); ; asm("is_motor_3:\n" ; "cmpb #$03\n" ; "bne is_motor_4\n" ; "ldx _new_motor_spd\n" ; "stx _motor3\n" ; "ldx _motor4\n" ; "stx _comp_motor_spd\n" ; "ldab #$01\n" ; "stab _first_motor\n" ; "jmp calc_motor_times\n" ; "is_motor_4:\n" ; "cmpb #$04\n" ; "bne is_motor_5\n" ; "ldx _new_motor_spd\n" ; "stx _motor4\n" ; "ldx _motor3\n" ; "stx _comp_motor_spd\n" ; "clr _first_motor\n" ; "jmp calc_motor_times"); ; asm("is_motor_5:\n" ; "cmpb #$05\n" ; "bne is_motor_6\n" ; "ldx _new_motor_spd\n" ; "stx _motor5\n" ; "ldx _motor6\n" ; "stx _comp_motor_spd\n" ; "ldab #$01\n" ; "stab _first_motor\n" ; "jmp calc_motor_times\n" ; "is_motor_6:\n" ; "cmpb #$06\n" ; "bne is_motor_7\n" ; "ldx _new_motor_spd\n" ; "stx _motor6\n" ; "ldx _motor5\n" ; "stx _comp_motor_spd\n" ; "clr _first_motor\n" ; "jmp calc_motor_times"); ; asm("is_motor_7:\n" ; "cmpb #$07\n" ; "bne is_motor_8\n" ; "ldx _new_motor_spd\n" ; "stx _motor7\n" ; "ldx _motor8\n" ; "stx _comp_motor_spd\n" ; "ldab #$01\n" ; "stab _first_motor\n" ; "jmp calc_motor_times\n" ; "is_motor_8:\n" ; "cmpb #$08\n" ; "bne end_routine\n" ; "ldx _new_motor_spd\n" ; "stx _motor8\n" ; "ldx _motor7\n" ; "stx _comp_motor_spd\n" ; "clr _first_motor\n" ; "bra calc_motor_times\n"); ; asm("end_routine:\n" ; "jmp multvlve_done\n" ; "calc_motor_times:\n" ; "clr _same_motor\n" ; "ldx _new_motor_spd\n" ; "cpx _comp_motor_spd\n" ; "bne calc_motor_nequ\n" ; "ldaa #$01\n" ; "staa _same_motor\n" ; "calc_motor_nequ:\n" ; "cpx _wave_period\n" ; "bne calc_mot_con1\n" ; "jmp clc_mot_40\n" ; "calc_mot_con1:\n" ; "cpx #$0000\n" ; "bne calc_mot_con2\n" ; "jmp clc_mot_00"); ; asm("calc_mot_con2:\n" ; "ldaa _same_motor\n" ; "bne calc_motor_same\n" ; "cpx _comp_motor_spd\n" ; "bls clc_mot_les\n" ; "jmp clc_mot_gt\n" ; "clc_mot_les:\n" ; "stx _calc_1vl\n" ; "ldd _comp_motor_spd\n" ; "subd _new_motor_spd\n" ; "std _calc_2vl\n" ; "ldd _wave_period\n" ; "cpd _comp_motor_spd\n" ; "beq clc_mot_lesc\n" ; "subd _calc_2vl\n" ; "subd _calc_1vl\n" ; "std _calc_3vl\n" ; "ldaa #0b00000011\n" ; "staa _calc_1ot\n" ; "ldaa #0b00000010\n" ; "staa _calc_2ot\n" ; "ldaa #0b00000000\n" ; "staa _calc_3ot\n" ; "jmp calc_mot_fin"); ; asm("clc_mot_lesc:\n" ; "subd _new_motor_spd\n" ; "subd #1000\n" ; "std _calc_3vl\n" ; "ldd #1000\n" ; "std _calc_2vl\n" ; "ldaa _first_motor\n" ; "bne is_mot_lessc\n" ; "ldaa #0b00000011\n" ; "staa _calc_1ot\n" ; "ldaa #0b00000001\n" ; "staa _calc_3ot\n" ; "staa _calc_2ot\n" ; "jmp calc_mot_fin\n" ; "is_mot_lessc:\n" ; "ldaa #0b00000011\n" ; "staa _calc_1ot\n" ; "ldaa #0b00000010\n" ; "staa _calc_3ot\n" ; "staa _calc_2ot\n" ; "jmp calc_mot_fin"); ; asm("calc_motor_same:\n" ; "ldd _new_motor_spd\n" ; "std _calc_1vl\n" ; "ldd #1000\n" ; "std _calc_2vl\n" ; "ldd _wave_period\n" ; "subd _calc_2vl\n" ; "subd _calc_1vl\n" ; "std _calc_3vl\n" ; "ldaa #0b00000011\n" ; "staa _calc_1ot\n" ; "ldaa #$00000000\n" ; "staa _calc_3ot\n" ; "staa _calc_2ot\n" ; "jmp calc_mot_fin"); ; asm("clc_mot_gt:\n" ; "ldd _comp_motor_spd\n" ; "cpd #$00\n" ; "beq clc_mot_gtr0\n" ; "std _calc_1vl\n" ; "ldd _new_motor_spd\n" ; "subd _comp_motor_spd\n" ; "std _calc_2vl\n" ; "ldd _wave_period\n" ; "subd _calc_2vl\n" ; "subd _calc_1vl\n" ; "std _calc_3vl\n" ; "ldaa _first_motor\n" ; "bne is_mot_gtrc\n" ; "ldaa #0b00000011\n" ; "staa _calc_1ot\n" ; "ldaa #0b00000010\n" ; "staa _calc_2ot\n" ; "ldaa #0b00000000\n" ; "staa _calc_3ot\n" ; "jmp calc_mot_fin") ; asm("is_mot_gtrc:\n" ; "ldaa #0b00000011\n" ; "staa _calc_1ot\n" ; "ldaa #0b00000001\n" ; "staa _calc_2ot\n" ; "ldaa #0b00000000\n" ; "staa _calc_3ot\n" ; "jmp calc_mot_fin\n" ; "clc_mot_gtr0:\n" ; "ldd _new_motor_spd\n" ; "std _calc_1vl\n" ; "ldd #1000\n" ; "std _calc_2vl\n" ; "ldd _wave_period\n" ; "subd _calc_2vl\n" ; "subd _calc_1vl\n" ; "std _calc_3vl\n" ; "ldaa _first_motor\n" ; "bne is_mot_gtrcc\n" ; "ldaa #0b00000010\n" ; "staa _calc_1ot\n" ; "ldaa #0b00000000\n" ; "staa _calc_3ot\n" ; "staa _calc_2ot\n" ; "jmp calc_mot_fin"); ; asm("is_mot_gtrcc:\n" ; "ldaa #0b00000001\n" ; "staa _calc_1ot\n" ; "ldaa #0b00000000\n" ; "staa _calc_3ot\n" ; "staa _calc_2ot\n" ; "jmp calc_mot_fin\n" ; "clc_mot_00:\n" ; "ldaa _same_motor\n" ; "bne both_zero\n" ; "ldd _comp_motor_spd\n" ; "cpd _wave_period\n" ; "beq clc_mot_0040\n" ; "std _calc_1vl\n" ; "ldd #1000\n" ; "std _calc_2vl\n" ; "ldd _wave_period\n" ; "subd _calc_2vl\n" ; "subd _calc_1vl\n" ; "std _calc_3vl\n" ; "ldaa _first_motor\n" ; "bne is_mot_00\n" ; "ldaa #0b00000001\n" ; "staa _calc_1ot\n" ; "ldaa #0b00000000\n" ; "staa _calc_3ot\n" ; "staa _calc_2ot\n" ; "jmp calc_mot_fin"); ; asm("is_mot_00:\n" ; "ldaa #0b00000010\n" ; "staa _calc_1ot\n" ; "ldaa #0b00000000\n" ; "staa _calc_3ot\n" ; "staa _calc_2ot\n" ; "jmp calc_mot_fin\n" ; "both_zero:\n" ; "ldaa #0b00000000\n" ; "staa _calc_1ot\n" ; "staa _calc_2ot\n" ; "staa _calc_3ot\n" ; "ldd #1000\n" ; "std _calc_1vl\n" ; "ldd #1000\n" ; "std _calc_2vl\n" ; "ldd _wave_period\n" ; "subd _calc_2vl\n" ; "subd _calc_1vl\n" ; "std _calc_3vl\n" ; "jmp calc_mot_fin\n" ; "clc_mot_0040:\n" ; "ldd #1000\n" ; "std _calc_1vl\n" ; "std _calc_2vl\n" ; "ldd _wave_period\n" ; "subd _calc_1vl\n" ; "subd _calc_2vl\n" ; "std _calc_3vl\n" ; "ldaa #0b00000010\n" ; "ldab _first_motor\n" ; "bne is_mot_0040\n" ; "ldaa #0b00000001"); ; asm("is_mot_0040:\n" ; "staa _calc_1ot\n" ; "staa _calc_2ot\n" ; "staa _calc_3ot\n" ; "jmp calc_mot_fin\n" ; "clc_mot_40:\n" ; "ldaa _same_motor\n" ; "bne both_40000\n" ; "ldd _comp_motor_spd\n" ; "cpd #$0000\n" ; "bne clc_mot_40c\n" ; "ldd #1000\n" ; "clc_mot_40c:\n" ; "std _calc_1vl\n" ; "ldd #1000\n" ; "std _calc_2vl\n" ; "ldd _wave_period\n" ; "subd _calc_2vl\n" ; "subd _calc_1vl\n" ; "std _calc_3vl\n" ; "ldaa _first_motor\n" ; "bne is_mot_40\n" ; "ldaa #0b00000011\n" ; "ldx _comp_motor_spd\n" ; "cpx #$0000\n" ; "bne clc_mot_40cc\n" ; "ldaa #0b00000010\n" ; "clc_mot_40cc:\n" ; "staa _calc_1ot\n" ; "ldaa #0b00000010\n" ; "staa _calc_3ot\n" ; "staa _calc_2ot\n" ; "jmp calc_mot_fin"); ; asm("is_mot_40:\n" ; "ldaa #0b00000011\n" ; "ldx _comp_motor_spd\n" ; "cpx #$0000\n" ; "bne is_mot_40c\n" ; "ldaa #0b00000001\n" ; "is_mot_40c:\n" ; "staa _calc_1ot\n" ; "ldaa #0b00000001\n" ; "staa _calc_3ot\n" ; "staa _calc_2ot\n" ; "jmp calc_mot_fin\n" ; "both_40000:\n" ; "ldd #1000\n" ; "std _calc_1vl\n" ; "ldd #1000\n" ; "std _calc_2vl\n" ; "ldd _wave_period\n" ; "subd _calc_2vl\n" ; "subd _calc_1vl\n" ; "std _calc_3vl\n" ; "ldaa #0b00000011\n" ; "staa _calc_1ot\n" ; "staa _calc_2ot\n" ; "staa _calc_3ot\n" ; "jmp calc_mot_fin"); ; asm("calc_mot_fin:\n" ; "ldy #_oc2_1ot\n" ; "ldaa _new_motor_num\n" ; "cmpa #$01\n" ; "beq jmp_calc_motor_done\n" ; "cmpa #$02\n" ; "beq jmp_calc_motor_done\n" ; "cmpa #$03\n" ; "beq calc_motor_34\n" ; "cmpa #$04\n" ; "beq calc_motor_34\n" ; "cmpa #$05\n" ; "beq calc_motor_56\n" ; "cmpa #$06\n" ; "beq calc_motor_56\n" ; "bra calc_motor_78\n" ; "jmp_calc_motor_done:\n" ; "jmp calc_motor_done"); ; asm("calc_motor_78:\n" ; "ldaa _calc_1ot\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "staa _calc_1ot\n" ; "ldaa _calc_2ot\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "staa _calc_2ot\n" ; "ldaa _calc_3ot\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "staa _calc_3ot\n" ; "ldy #_oc5_1ot\n" ; "jmp calc_motor_done"); ; asm("calc_motor_56:\n" ; "ldaa _calc_1ot\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "staa _calc_1ot\n" ; "ldaa _calc_2ot\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "staa _calc_2ot\n" ; "ldaa _calc_3ot\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "lsla\n" ; "staa _calc_3ot\n" ; "ldy #_oc4_1ot\n" ; "jmp calc_motor_done"); ; asm("calc_motor_34:\n" ; "ldaa _calc_1ot\n" ; "lsla\n" ; "lsla\n" ; "staa _calc_1ot\n" ; "ldaa _calc_2ot\n" ; "lsla\n" ; "lsla\n" ; "staa _calc_2ot\n" ; "ldaa _calc_3ot\n" ; "lsla\n" ; "lsla\n" ; "staa _calc_3ot\n" ; "ldy #_oc3_1ot\n" ; "calc_motor_done:\n" ; "ldx #_calc_1ot\n" ; "ldaa 0,x\n" ; "staa 0,y\n" ; "inx\n" ; "iny\n" ; "ldd 0,x\n" ; "std 0,y\n" ; "inx\n" ; "inx\n" ; "iny\n" ; "iny\n" ; "ldaa 0,x\n" ; "staa 0,y\n" ; "inx\n" ; "iny\n" ; "ldd 0,x\n" ; "std 0,y\n" ; "inx\n" ; "inx\n" ; "iny\n" ; "iny\n" ; "ldaa 0,x\n" ; "staa 0,y\n" ; "inx\n" ; "iny\n" ; "ldd 0,x\n" ; "std 0,y\n" ; "multvlve_done:\n" ; "ldx _x_preserve"); ; ; ; } 0460 L5: 0460 08 inx 0461 08 inx 0462 35 txs 0463 38 pulx 0464 1838 puly 0466 39 rts ; IX -> 0,x ; ?temp -> 2,x ; ?temp -> 4,x ; ?temp -> 6,x ; ?temp -> 8,x ; ?temp -> 10,x ; ?temp -> 12,x ; ?temp -> 14,x ; ?temp -> 16,x ; ?temp -> 18,x ; ?temp -> 20,x ; ?temp -> 22,x ; ?temp -> 24,x ; ?temp -> 26,x ; ?temp -> 28,x ; ?temp -> 30,x ; ?temp -> 32,x ; derivative -> 34,x ; integral -> 36,x ; proportion -> 38,x ; new_duty -> 40,x 0467 _pid_controller:: 0467 BD0000 jsr __enterb 046A 2A .byte 0x2a 046B .dbfunc pid_controller 046B .dbline 655 ; ; /********************************************************/ ; /*pid_controller() */ ; /*function contains all the pid routines to update */ ; /*the duty cycles */ ; /* */ ; /********************************************************/ ; ; void pid_controller(void){ ; ; int proportion = 0; 046B CC0000 ldd #0 046E ED26 std 38,x 0470 .dbline 656 ; int integral = 0; 0470 CC0000 ldd #0 0473 ED24 std 36,x 0475 .dbline 657 ; int derivative = 0; 0475 CC0000 ldd #0 0478 ED22 std 34,x 047A .dbline 658 ; int new_duty = 0; 047A CC0000 ldd #0 047D ED28 std 40,x 047F .dbline 664 ; ; ; //Run pid for cylinder 1 ; ; //shift error values ; error1[2] = error1[1]; 047F FC0200 ldd _error1+2 0482 FD0202 std _error1+4 0485 .dbline 665 ; error1[1] = error1[0]; 0485 FC01FE ldd _error1 0488 FD0200 std _error1+2 048B .dbline 666 ; error1[0] = cyl1_des - cyl1_pstn; 048B FC01EE ldd _cyl1_des 048E B301DE subd _cyl1_pstn 0491 FD01FE std _error1 0494 .dbline 669 ; ; //range for tolerance ; if (error1[0] < TOLERANCE & error1[0] > 0 - TOLERANCE){ 0494 FC01FE ldd _error1 0497 1A830004 cpd #4 049B 2C07 bge L14 049D CC0001 ldd #1 04A0 ED20 std 32,x 04A2 2005 bra L15 04A4 L14: 04A4 CC0000 ldd #0 04A7 ED20 std 32,x 04A9 L15: 04A9 FC01FE ldd _error1 04AC 1A83FFFC cpd #-4 04B0 2F07 ble L16 04B2 CC0001 ldd #1 04B5 ED1E std 30,x 04B7 2005 bra L17 04B9 L16: 04B9 CC0000 ldd #0 04BC ED1E std 30,x 04BE L17: 04BE EC20 ldd 32,x 04C0 A41E anda 30,x 04C2 E41F andb 31,x 04C4 1A830000 cpd #0 04C8 2715 beq L10 04CA .dbline 670 ; error1[0] = 0; 04CA CC0000 ldd #0 04CD FD01FE std _error1 04D0 .dbline 671 ; multvlve(0, 0x01); 04D0 CC0001 ldd #1 04D3 37 pshb 04D4 36 psha 04D5 CC0000 ldd #0 04D8 BD0069 jsr _multvlve 04DB 1838 puly 04DD .dbline 672 ; } 04DD 205E bra L11 04DF L10: 04DF .dbline 676 ; else { //run pid ; ; //calc gains ; proportion = K1[P] * error1[0]; 04DF FC022E ldd _K1 04E2 18FE01FE ldy _error1 04E6 BD0000 jsr __muli 04E9 ED26 std 38,x 04EB .dbline 681 ; //integral = K1[I]; ; //derivative = K1[D] * (error1[0] - error1[1]); ; ; //calc new raw value ; new_duty = proportion + integral + derivative; 04EB EC26 ldd 38,x 04ED E324 addd 36,x 04EF E322 addd 34,x 04F1 ED28 std 40,x 04F3 .dbline 684 ; ; ; if (error1[0] < 0) { 04F3 FC01FE ldd _error1 04F6 2C0A bge L18 04F8 .dbline 685 ; DIR_PORT &= 0xFE; 04F8 18CE4000 ldy #0x4000 04FC 181D0001 bclr 0,y,#0x1 0500 .dbline 686 ; } 0500 2008 bra L19 0502 L18: 0502 .dbline 688 ; else { ; DIR_PORT |= 0x01; 0502 18CE4000 ldy #0x4000 0506 181C0001 bset 0,y,#1 050A .dbline 689 ; } 050A L19: 050A .dbline 692 ; ; //take absolute value of new_duty ; if (new_duty < 0){ 050A EC28 ldd 40,x 050C 2C09 bge L20 050E EC28 ldd 40,x 0510 830001 subd #1 0513 43 coma 0514 53 comb 0515 ED28 std 40,x 0517 .dbline 698 ; asm("ldd %new_duty\n" ; "subd #1\n" ; "coma\n" ; "comb\n" ; "std %new_duty\n"); ; } 0517 L20: 0517 .dbline 700 ; //range new_duty to max value ; if (new_duty > 236){ 0517 EC28 ldd 40,x 0519 1A8300EC cpd #236 051D 2F05 ble L22 051F .dbline 701 ; new_duty = 236; 051F CC00EC ldd #236 0522 ED28 std 40,x 0524 .dbline 702 ; } 0524 L22: 0524 .dbline 704 ; ; new_duty = pwm_vals[new_duty]; 0524 EC28 ldd 40,x 0526 05 lsld 0527 C30000 addd #_pwm_vals 052A 188F xgdy 052C 18EC00 ldd 0,y 052F ED28 std 40,x 0531 .dbline 707 ; ; ; multvlve(new_duty, 0x01); 0531 CC0001 ldd #1 0534 37 pshb 0535 36 psha 0536 EC28 ldd 40,x 0538 BD0069 jsr _multvlve 053B 1838 puly 053D .dbline 709 ; ; } 053D L11: 053D .dbline 714 ; ; //Run pid for cylinder 2 ; ; //shift error values ; error2[2] = error2[1]; 053D FC0206 ldd _error2+2 0540 FD0208 std _error2+4 0543 .dbline 715 ; error2[1] = error2[0]; 0543 FC0204 ldd _error2 0546 FD0206 std _error2+2 0549 .dbline 716 ; error2[0] = cyl2_des - cyl2_pstn; 0549 FC01F0 ldd _cyl2_des 054C B301E0 subd _cyl2_pstn 054F FD0204 std _error2 0552 .dbline 719 ; ; //range for tolerance ; if (error2[0] < TOLERANCE & error2[0] > 0 - TOLERANCE){ 0552 FC0204 ldd _error2 0555 1A830004 cpd #4 0559 2C07 bge L31 055B CC0001 ldd #1 055E ED1C std 28,x 0560 2005 bra L32 0562 L31: 0562 CC0000 ldd #0 0565 ED1C std 28,x 0567 L32: 0567 FC0204 ldd _error2 056A 1A83FFFC cpd #-4 056E 2F07 ble L33 0570 CC0001 ldd #1 0573 ED1A std 26,x 0575 2005 bra L34 0577 L33: 0577 CC0000 ldd #0 057A ED1A std 26,x 057C L34: 057C EC1C ldd 28,x 057E A41A anda 26,x 0580 E41B andb 27,x 0582 1A830000 cpd #0 0586 2715 beq L27 0588 .dbline 720 ; error2[0] = 0; 0588 CC0000 ldd #0 058B FD0204 std _error2 058E .dbline 721 ; multvlve(0, 0x02); 058E CC0002 ldd #2 0591 37 pshb 0592 36 psha 0593 CC0000 ldd #0 0596 BD0069 jsr _multvlve 0599 1838 puly 059B .dbline 722 ; } 059B 205E bra L28 059D L27: 059D .dbline 726 ; else { //run pid ; ; //calc gains ; proportion = K2[P] * error2[0]; 059D FC0234 ldd _K2 05A0 18FE0204 ldy _error2 05A4 BD0000 jsr __muli 05A7 ED26 std 38,x 05A9 .dbline 731 ; //integral = K2[I]; ; //derivative = K2[D] * (error2[0] - error2[1]); ; ; //calc new raw value ; new_duty = proportion + integral + derivative; 05A9 EC26 ldd 38,x 05AB E324 addd 36,x 05AD E322 addd 34,x 05AF ED28 std 40,x 05B1 .dbline 734 ; ; ; if (error2[0] < 0) { 05B1 FC0204 ldd _error2 05B4 2C0A bge L35 05B6 .dbline 735 ; DIR_PORT &= 0xFD; 05B6 18CE4000 ldy #0x4000 05BA 181D0002 bclr 0,y,#0x2 05BE .dbline 736 ; } 05BE 2008 bra L36 05C0 L35: 05C0 .dbline 738 ; else { ; DIR_PORT |= 0x02; 05C0 18CE4000 ldy #0x4000 05C4 181C0002 bset 0,y,#2 05C8 .dbline 739 ; } 05C8 L36: 05C8 .dbline 742 ; ; //take absolute value of new_duty ; if (new_duty < 0){ 05C8 EC28 ldd 40,x 05CA 2C09 bge L37 05CC EC28 ldd 40,x 05CE 830001 subd #1 05D1 43 coma 05D2 53 comb 05D3 ED28 std 40,x 05D5 .dbline 748 ; asm("ldd %new_duty\n" ; "subd #1\n" ; "coma\n" ; "comb\n" ; "std %new_duty\n"); ; } 05D5 L37: 05D5 .dbline 750 ; //range new_duty to max value ; if (new_duty > 236){ 05D5 EC28 ldd 40,x 05D7 1A8300EC cpd #236 05DB 2F05 ble L39 05DD .dbline 751 ; new_duty = 236; 05DD CC00EC ldd #236 05E0 ED28 std 40,x 05E2 .dbline 752 ; } 05E2 L39: 05E2 .dbline 754 ; ; new_duty = pwm_vals[new_duty]; 05E2 EC28 ldd 40,x 05E4 05 lsld 05E5 C30000 addd #_pwm_vals 05E8 188F xgdy 05EA 18EC00 ldd 0,y 05ED ED28 std 40,x 05EF .dbline 757 ; ; ; multvlve(new_duty, 0x02); 05EF CC0002 ldd #2 05F2 37 pshb 05F3 36 psha 05F4 EC28 ldd 40,x 05F6 BD0069 jsr _multvlve 05F9 1838 puly 05FB .dbline 759 ; ; } 05FB L28: 05FB .dbline 765 ; ; ; //Run pid for cylinder 3 ; ; //shift error values ; error3[2] = error3[1]; 05FB FC020C ldd _error3+2 05FE FD020E std _error3+4 0601 .dbline 766 ; error3[1] = error3[0]; 0601 FC020A ldd _error3 0604 FD020C std _error3+2 0607 .dbline 767 ; error3[0] = cyl3_des - cyl3_pstn; 0607 FC01F2 ldd _cyl3_des 060A B301E2 subd _cyl3_pstn 060D FD020A std _error3 0610 .dbline 770 ; ; //range for tolerance ; if (error3[0] < TOLERANCE & error3[0] > 0 - TOLERANCE){ 0610 FC020A ldd _error3 0613 1A830004 cpd #4 0617 2C07 bge L48 0619 CC0001 ldd #1 061C ED18 std 24,x 061E 2005 bra L49 0620 L48: 0620 CC0000 ldd #0 0623 ED18 std 24,x 0625 L49: 0625 FC020A ldd _error3 0628 1A83FFFC cpd #-4 062C 2F07 ble L50 062E CC0001 ldd #1 0631 ED16 std 22,x 0633 2005 bra L51 0635 L50: 0635 CC0000 ldd #0 0638 ED16 std 22,x 063A L51: 063A EC18 ldd 24,x 063C A416 anda 22,x 063E E417 andb 23,x 0640 1A830000 cpd #0 0644 2715 beq L44 0646 .dbline 771 ; error3[0] = 0; 0646 CC0000 ldd #0 0649 FD020A std _error3 064C .dbline 772 ; multvlve(0, 0x03); 064C CC0003 ldd #3 064F 37 pshb 0650 36 psha 0651 CC0000 ldd #0 0654 BD0069 jsr _multvlve 0657 1838 puly 0659 .dbline 773 ; } 0659 205E bra L45 065B L44: 065B .dbline 777 ; else { //run pid ; ; //calc gains ; proportion = K3[P] * error3[0]; 065B FC023A ldd _K3 065E 18FE020A ldy _error3 0662 BD0000 jsr __muli 0665 ED26 std 38,x 0667 .dbline 782 ; //integral = K3[I]; ; //derivative = K3[D] * (error3[0] - error3[1]); ; ; //calc new raw value ; new_duty = proportion + integral + derivative; 0667 EC26 ldd 38,x 0669 E324 addd 36,x 066B E322 addd 34,x 066D ED28 std 40,x 066F .dbline 785 ; ; ; if (error3[0] < 0) { 066F FC020A ldd _error3 0672 2C0A bge L52 0674 .dbline 786 ; DIR_PORT &= 0xFB; 0674 18CE4000 ldy #0x4000 0678 181D0004 bclr 0,y,#0x4 067C .dbline 787 ; } 067C 2008 bra L53 067E L52: 067E .dbline 789 ; else { ; DIR_PORT |= 0x04; 067E 18CE4000 ldy #0x4000 0682 181C0004 bset 0,y,#4 0686 .dbline 790 ; } 0686 L53: 0686 .dbline 793 ; ; //take absolute value of new_duty ; if (new_duty < 0){ 0686 EC28 ldd 40,x 0688 2C09 bge L54 068A EC28 ldd 40,x 068C 830001 subd #1 068F 43 coma 0690 53 comb 0691 ED28 std 40,x 0693 .dbline 799 ; asm("ldd %new_duty\n" ; "subd #1\n" ; "coma\n" ; "comb\n" ; "std %new_duty\n"); ; } 0693 L54: 0693 .dbline 801 ; //range new_duty to max value ; if (new_duty > 236){ 0693 EC28 ldd 40,x 0695 1A8300EC cpd #236 0699 2F05 ble L56 069B .dbline 802 ; new_duty = 236; 069B CC00EC ldd #236 069E ED28 std 40,x 06A0 .dbline 803 ; } 06A0 L56: 06A0 .dbline 805 ; ; new_duty = pwm_vals[new_duty]; 06A0 EC28 ldd 40,x 06A2 05 lsld 06A3 C30000 addd #_pwm_vals 06A6 188F xgdy 06A8 18EC00 ldd 0,y 06AB ED28 std 40,x 06AD .dbline 808 ; ; ; multvlve(new_duty, 0x03); 06AD CC0003 ldd #3 06B0 37 pshb 06B1 36 psha 06B2 EC28 ldd 40,x 06B4 BD0069 jsr _multvlve 06B7 1838 puly 06B9 .dbline 810 ; ; } 06B9 L45: 06B9 .dbline 817 ; ; ; ; //Run pid for cylinder 4 ; ; //shift error values ; error4[2] = error4[1]; 06B9 FC0212 ldd _error4+2 06BC FD0214 std _error4+4 06BF .dbline 818 ; error4[1] = error4[0]; 06BF FC0210 ldd _error4 06C2 FD0212 std _error4+2 06C5 .dbline 819 ; error4[0] = cyl4_des - cyl4_pstn; 06C5 FC01F4 ldd _cyl4_des 06C8 B301E4 subd _cyl4_pstn 06CB FD0210 std _error4 06CE .dbline 822 ; ; //range for tolerance ; if (error4[0] < TOLERANCE & error4[0] > 0 - TOLERANCE){ 06CE FC0210 ldd _error4 06D1 1A830004 cpd #4 06D5 2C07 bge L65 06D7 CC0001 ldd #1 06DA ED14 std 20,x 06DC 2005 bra L66 06DE L65: 06DE CC0000 ldd #0 06E1 ED14 std 20,x 06E3 L66: 06E3 FC0210 ldd _error4 06E6 1A83FFFC cpd #-4 06EA 2F07 ble L67 06EC CC0001 ldd #1 06EF ED12 std 18,x 06F1 2005 bra L68 06F3 L67: 06F3 CC0000 ldd #0 06F6 ED12 std 18,x 06F8 L68: 06F8 EC14 ldd 20,x 06FA A412 anda 18,x 06FC E413 andb 19,x 06FE 1A830000 cpd #0 0702 2715 beq L61 0704 .dbline 823 ; error4[0] = 0; 0704 CC0000 ldd #0 0707 FD0210 std _error4 070A .dbline 824 ; multvlve(0, 0x04); 070A CC0004 ldd #4 070D 37 pshb 070E 36 psha 070F CC0000 ldd #0 0712 BD0069 jsr _multvlve 0715 1838 puly 0717 .dbline 825 ; } 0717 205E bra L62 0719 L61: 0719 .dbline 829 ; else { //run pid ; ; //calc gains ; proportion = K4[P] * error4[0]; 0719 FC0240 ldd _K4 071C 18FE0210 ldy _error4 0720 BD0000 jsr __muli 0723 ED26 std 38,x 0725 .dbline 834 ; //integral = K4[I]; ; //derivative = K4[D] * (error4[0] - error4[1]); ; ; //calc new raw value ; new_duty = proportion + integral + derivative; 0725 EC26 ldd 38,x 0727 E324 addd 36,x 0729 E322 addd 34,x 072B ED28 std 40,x 072D .dbline 837 ; ; ; if (error4[0] < 0) { 072D FC0210 ldd _error4 0730 2C0A bge L69 0732 .dbline 838 ; DIR_PORT &= 0xF7; 0732 18CE4000 ldy #0x4000 0736 181D0008 bclr 0,y,#0x8 073A .dbline 839 ; } 073A 2008 bra L70 073C L69: 073C .dbline 841 ; else { ; DIR_PORT |= 0x08; 073C 18CE4000 ldy #0x4000 0740 181C0008 bset 0,y,#8 0744 .dbline 842 ; } 0744 L70: 0744 .dbline 845 ; ; //take absolute value of new_duty ; if (new_duty < 0){ 0744 EC28 ldd 40,x 0746 2C09 bge L71 0748 EC28 ldd 40,x 074A 830001 subd #1 074D 43 coma 074E 53 comb 074F ED28 std 40,x 0751 .dbline 851 ; asm("ldd %new_duty\n" ; "subd #1\n" ; "coma\n" ; "comb\n" ; "std %new_duty\n"); ; } 0751 L71: 0751 .dbline 853 ; //range new_duty to max value ; if (new_duty > 236){ 0751 EC28 ldd 40,x 0753 1A8300EC cpd #236 0757 2F05 ble L73 0759 .dbline 854 ; new_duty = 236; 0759 CC00EC ldd #236 075C ED28 std 40,x 075E .dbline 855 ; } 075E L73: 075E .dbline 857 ; ; new_duty = pwm_vals[new_duty]; 075E EC28 ldd 40,x 0760 05 lsld 0761 C30000 addd #_pwm_vals 0764 188F xgdy 0766 18EC00 ldd 0,y 0769 ED28 std 40,x 076B .dbline 860 ; ; ; multvlve(new_duty, 0x04); 076B CC0004 ldd #4 076E 37 pshb 076F 36 psha 0770 EC28 ldd 40,x 0772 BD0069 jsr _multvlve 0775 1838 puly 0777 .dbline 862 ; ; } 0777 L62: 0777 .dbline 868 ; ; ; //Run pid for cylinder 5 ; ; //shift error values ; error5[2] = error5[1]; 0777 FC0218 ldd _error5+2 077A FD021A std _error5+4 077D .dbline 869 ; error5[1] = error5[0]; 077D FC0216 ldd _error5 0780 FD0218 std _error5+2 0783 .dbline 870 ; error5[0] = cyl5_des - cyl5_pstn; 0783 FC01F6 ldd _cyl5_des 0786 B301E6 subd _cyl5_pstn 0789 FD0216 std _error5 078C .dbline 873 ; ; //range for tolerance ; if (error5[0] < TOLERANCE & error5[0] > 0 - TOLERANCE){ 078C FC0216 ldd _error5 078F 1A830004 cpd #4 0793 2C07 bge L82 0795 CC0001 ldd #1 0798 ED10 std 16,x 079A 2005 bra L83 079C L82: 079C CC0000 ldd #0 079F ED10 std 16,x 07A1 L83: 07A1 FC0216 ldd _error5 07A4 1A83FFFC cpd #-4 07A8 2F07 ble L84 07AA CC0001 ldd #1 07AD ED0E std 14,x 07AF 2005 bra L85 07B1 L84: 07B1 CC0000 ldd #0 07B4 ED0E std 14,x 07B6 L85: 07B6 EC10 ldd 16,x 07B8 A40E anda 14,x 07BA E40F andb 15,x 07BC 1A830000 cpd #0 07C0 2715 beq L78 07C2 .dbline 874 ; error5[0] = 0; 07C2 CC0000 ldd #0 07C5 FD0216 std _error5 07C8 .dbline 875 ; multvlve(0, 0x05); 07C8 CC0005 ldd #5 07CB 37 pshb 07CC 36 psha 07CD CC0000 ldd #0 07D0 BD0069 jsr _multvlve 07D3 1838 puly 07D5 .dbline 876 ; } 07D5 205E bra L79 07D7 L78: 07D7 .dbline 880 ; else { //run pid ; ; //calc gains ; proportion = K5[P] * error5[0]; 07D7 FC0246 ldd _K5 07DA 18FE0216 ldy _error5 07DE BD0000 jsr __muli 07E1 ED26 std 38,x 07E3 .dbline 885 ; //integral = K5[I]; ; //derivative = K5[D] * (error5[0] - error5[1]); ; ; //calc new raw value ; new_duty = proportion + integral + derivative; 07E3 EC26 ldd 38,x 07E5 E324 addd 36,x 07E7 E322 addd 34,x 07E9 ED28 std 40,x 07EB .dbline 888 ; ; ; if (error5[0] < 0) { 07EB FC0216 ldd _error5 07EE 2C0A bge L86 07F0 .dbline 889 ; DIR_PORT &= 0xEF; 07F0 18CE4000 ldy #0x4000 07F4 181D0010 bclr 0,y,#0x10 07F8 .dbline 890 ; } 07F8 2008 bra L87 07FA L86: 07FA .dbline 892 ; else { ; DIR_PORT |= 0x10; 07FA 18CE4000 ldy #0x4000 07FE 181C0010 bset 0,y,#16 0802 .dbline 893 ; } 0802 L87: 0802 .dbline 896 ; ; //take absolute value of new_duty ; if (new_duty < 0){ 0802 EC28 ldd 40,x 0804 2C09 bge L88 0806 EC28 ldd 40,x 0808 830001 subd #1 080B 43 coma 080C 53 comb 080D ED28 std 40,x 080F .dbline 902 ; asm("ldd %new_duty\n" ; "subd #1\n" ; "coma\n" ; "comb\n" ; "std %new_duty\n"); ; } 080F L88: 080F .dbline 904 ; //range new_duty to max value ; if (new_duty > 236){ 080F EC28 ldd 40,x 0811 1A8300EC cpd #236 0815 2F05 ble L90 0817 .dbline 905 ; new_duty = 236; 0817 CC00EC ldd #236 081A ED28 std 40,x 081C .dbline 906 ; } 081C L90: 081C .dbline 908 ; ; new_duty = pwm_vals[new_duty]; 081C EC28 ldd 40,x 081E 05 lsld 081F C30000 addd #_pwm_vals 0822 188F xgdy 0824 18EC00 ldd 0,y 0827 ED28 std 40,x 0829 .dbline 911 ; ; ; multvlve(new_duty, 0x05); 0829 CC0005 ldd #5 082C 37 pshb 082D 36 psha 082E EC28 ldd 40,x 0830 BD0069 jsr _multvlve 0833 1838 puly 0835 .dbline 913 ; ; } 0835 L79: 0835 .dbline 919 ; ; ; //Run pid for cylinder 6 ; ; //shift error values ; error6[2] = error6[1]; 0835 FC021E ldd _error6+2 0838 FD0220 std _error6+4 083B .dbline 920 ; error6[1] = error6[0]; 083B FC021C ldd _error6 083E FD021E std _error6+2 0841 .dbline 921 ; error6[0] = cyl6_des - cyl6_pstn; 0841 FC01F8 ldd _cyl6_des 0844 B301E8 subd _cyl6_pstn 0847 FD021C std _error6 084A .dbline 924 ; ; //range for tolerance ; if (error6[0] < TOLERANCE & error6[0] > 0 - TOLERANCE){ 084A FC021C ldd _error6 084D 1A830004 cpd #4 0851 2C07 bge L99 0853 CC0001 ldd #1 0856 ED0C std 12,x 0858 2005 bra L100 085A L99: 085A CC0000 ldd #0 085D ED0C std 12,x 085F L100: 085F FC021C ldd _error6 0862 1A83FFFC cpd #-4 0866 2F07 ble L101 0868 CC0001 ldd #1 086B ED0A std 10,x 086D 2005 bra L102 086F L101: 086F CC0000 ldd #0 0872 ED0A std 10,x 0874 L102: 0874 EC0C ldd 12,x 0876 A40A anda 10,x 0878 E40B andb 11,x 087A 1A830000 cpd #0 087E 2715 beq L95 0880 .dbline 925 ; error6[0] = 0; 0880 CC0000 ldd #0 0883 FD021C std _error6 0886 .dbline 926 ; multvlve(0, 0x06); 0886 CC0006 ldd #6 0889 37 pshb 088A 36 psha 088B CC0000 ldd #0 088E BD0069 jsr _multvlve 0891 1838 puly 0893 .dbline 927 ; } 0893 205E bra L96 0895 L95: 0895 .dbline 931 ; else { //run pid ; ; //calc gains ; proportion = K6[P] * error6[0]; 0895 FC024C ldd _K6 0898 18FE021C ldy _error6 089C BD0000 jsr __muli 089F ED26 std 38,x 08A1 .dbline 936 ; //integral = K6[I]; ; //derivative = K6[D] * (error6[0] - error6[1]); ; ; //calc new raw value ; new_duty = proportion + integral + derivative; 08A1 EC26 ldd 38,x 08A3 E324 addd 36,x 08A5 E322 addd 34,x 08A7 ED28 std 40,x 08A9 .dbline 939 ; ; ; if (error6[0] < 0) { 08A9 FC021C ldd _error6 08AC 2C0A bge L103 08AE .dbline 940 ; DIR_PORT &= 0xDF; 08AE 18CE4000 ldy #0x4000 08B2 181D0020 bclr 0,y,#0x20 08B6 .dbline 941 ; } 08B6 2008 bra L104 08B8 L103: 08B8 .dbline 943 ; else { ; DIR_PORT |= 0x20; 08B8 18CE4000 ldy #0x4000 08BC 181C0020 bset 0,y,#32 08C0 .dbline 944 ; } 08C0 L104: 08C0 .dbline 947 ; ; //take absolute value of new_duty ; if (new_duty < 0){ 08C0 EC28 ldd 40,x 08C2 2C09 bge L105 08C4 EC28 ldd 40,x 08C6 830001 subd #1 08C9 43 coma 08CA 53 comb 08CB ED28 std 40,x 08CD .dbline 953 ; asm("ldd %new_duty\n" ; "subd #1\n" ; "coma\n" ; "comb\n" ; "std %new_duty\n"); ; } 08CD L105: 08CD .dbline 955 ; //range new_duty to max value ; if (new_duty > 236){ 08CD EC28 ldd 40,x 08CF 1A8300EC cpd #236 08D3 2F05 ble L107 08D5 .dbline 956 ; new_duty = 236; 08D5 CC00EC ldd #236 08D8 ED28 std 40,x 08DA .dbline 957 ; } 08DA L107: 08DA .dbline 959 ; ; new_duty = pwm_vals[new_duty]; 08DA EC28 ldd 40,x 08DC 05 lsld 08DD C30000 addd #_pwm_vals 08E0 188F xgdy 08E2 18EC00 ldd 0,y 08E5 ED28 std 40,x 08E7 .dbline 962 ; ; ; multvlve(new_duty, 0x06); 08E7 CC0006 ldd #6 08EA 37 pshb 08EB 36 psha 08EC EC28 ldd 40,x 08EE BD0069 jsr _multvlve 08F1 1838 puly 08F3 .dbline 964 ; ; } 08F3 L96: 08F3 .dbline 971 ; ; ; ; //Run pid for cylinder 7 ; ; //shift error values ; error7[2] = error7[1]; 08F3 FC0224 ldd _error7+2 08F6 FD0226 std _error7+4 08F9 .dbline 972 ; error7[1] = error7[0]; 08F9 FC0222 ldd _error7 08FC FD0224 std _error7+2 08FF .dbline 973 ; error7[0] = cyl7_des - cyl7_pstn; 08FF FC01FA ldd _cyl7_des 0902 B301EA subd _cyl7_pstn 0905 FD0222 std _error7 0908 .dbline 976 ; ; //range for tolerance ; if (error7[0] < TOLERANCE & error7[0] > 0 - TOLERANCE){ 0908 FC0222 ldd _error7 090B 1A830004 cpd #4 090F 2C07 bge L116 0911 CC0001 ldd #1 0914 ED08 std 8,x 0916 2005 bra L117 0918 L116: 0918 CC0000 ldd #0 091B ED08 std 8,x 091D L117: 091D FC0222 ldd _error7 0920 1A83FFFC cpd #-4 0924 2F07 ble L118 0926 CC0001 ldd #1 0929 ED06 std 6,x 092B 2005 bra L119 092D L118: 092D CC0000 ldd #0 0930 ED06 std 6,x 0932 L119: 0932 EC08 ldd 8,x 0934 A406 anda 6,x 0936 E407 andb 7,x 0938 1A830000 cpd #0 093C 2715 beq L112 093E .dbline 977 ; error7[0] = 0; 093E CC0000 ldd #0 0941 FD0222 std _error7 0944 .dbline 978 ; multvlve(0, 0x07); 0944 CC0007 ldd #7 0947 37 pshb 0948 36 psha 0949 CC0000 ldd #0 094C BD0069 jsr _multvlve 094F 1838 puly 0951 .dbline 979 ; } 0951 205E bra L113 0953 L112: 0953 .dbline 983 ; else { //run pid ; ; //calc gains ; proportion = K7[P] * error7[0]; 0953 FC0252 ldd _K7 0956 18FE0222 ldy _error7 095A BD0000 jsr __muli 095D ED26 std 38,x 095F .dbline 988 ; //integral = K7[I]; ; //derivative = K7[D] * (error7[0] - error7[1]); ; ; //calc new raw value ; new_duty = proportion + integral + derivative; 095F EC26 ldd 38,x 0961 E324 addd 36,x 0963 E322 addd 34,x 0965 ED28 std 40,x 0967 .dbline 991 ; ; ; if (error7[0] < 0) { 0967 FC0222 ldd _error7 096A 2C0A bge L120 096C .dbline 992 ; DIR_PORT &= 0xBF; 096C 18CE4000 ldy #0x4000 0970 181D0040 bclr 0,y,#0x40 0974 .dbline 993 ; } 0974 2008 bra L121 0976 L120: 0976 .dbline 995 ; else { ; DIR_PORT |= 0x40; 0976 18CE4000 ldy #0x4000 097A 181C0040 bset 0,y,#64 097E .dbline 996 ; } 097E L121: 097E .dbline 999 ; ; //take absolute value of new_duty ; if (new_duty < 0){ 097E EC28 ldd 40,x 0980 2C09 bge L122 0982 EC28 ldd 40,x 0984 830001 subd #1 0987 43 coma 0988 53 comb 0989 ED28 std 40,x 098B .dbline 1005 ; asm("ldd %new_duty\n" ; "subd #1\n" ; "coma\n" ; "comb\n" ; "std %new_duty\n"); ; } 098B L122: 098B .dbline 1007 ; //range new_duty to max value ; if (new_duty > 236){ 098B EC28 ldd 40,x 098D 1A8300EC cpd #236 0991 2F05 ble L124 0993 .dbline 1008 ; new_duty = 236; 0993 CC00EC ldd #236 0996 ED28 std 40,x 0998 .dbline 1009 ; } 0998 L124: 0998 .dbline 1011 ; ; new_duty = pwm_vals[new_duty]; 0998 EC28 ldd 40,x 099A 05 lsld 099B C30000 addd #_pwm_vals 099E 188F xgdy 09A0 18EC00 ldd 0,y 09A3 ED28 std 40,x 09A5 .dbline 1014 ; ; ; multvlve(new_duty, 0x07); 09A5 CC0007 ldd #7 09A8 37 pshb 09A9 36 psha 09AA EC28 ldd 40,x 09AC BD0069 jsr _multvlve 09AF 1838 puly 09B1 .dbline 1016 ; ; } 09B1 L113: 09B1 .dbline 1023 ; ; ; ; //Run pid for cylinder 8 ; ; //shift error values ; error8[2] = error8[1]; 09B1 FC022A ldd _error8+2 09B4 FD022C std _error8+4 09B7 .dbline 1024 ; error8[1] = error8[0]; 09B7 FC0228 ldd _error8 09BA FD022A std _error8+2 09BD .dbline 1025 ; error8[0] = cyl8_des - cyl8_pstn; 09BD FC01FC ldd _cyl8_des 09C0 B301EC subd _cyl8_pstn 09C3 FD0228 std _error8 09C6 .dbline 1028 ; ; //range for tolerance ; if (error8[0] < TOLERANCE & error8[0] > 0 - TOLERANCE){ 09C6 FC0228 ldd _error8 09C9 1A830004 cpd #4 09CD 2C07 bge L133 09CF CC0001 ldd #1 09D2 ED04 std 4,x 09D4 2005 bra L134 09D6 L133: 09D6 CC0000 ldd #0 09D9 ED04 std 4,x 09DB L134: 09DB FC0228 ldd _error8 09DE 1A83FFFC cpd #-4 09E2 2F07 ble L135 09E4 CC0001 ldd #1 09E7 ED02 std 2,x 09E9 2005 bra L136 09EB L135: 09EB CC0000 ldd #0 09EE ED02 std 2,x 09F0 L136: 09F0 EC04 ldd 4,x 09F2 A402 anda 2,x 09F4 E403 andb 3,x 09F6 1A830000 cpd #0 09FA 2715 beq L129 09FC .dbline 1029 ; error8[0] = 0; 09FC CC0000 ldd #0 09FF FD0228 std _error8 0A02 .dbline 1030 ; multvlve(0, 0x08); 0A02 CC0008 ldd #8 0A05 37 pshb 0A06 36 psha 0A07 CC0000 ldd #0 0A0A BD0069 jsr _multvlve 0A0D 1838 puly 0A0F .dbline 1031 ; } 0A0F 205E bra L130 0A11 L129: 0A11 .dbline 1035 ; else { //run pid ; ; //calc gains ; proportion = K8[P] * error8[0]; 0A11 FC0258 ldd _K8 0A14 18FE0228 ldy _error8 0A18 BD0000 jsr __muli 0A1B ED26 std 38,x 0A1D .dbline 1040 ; //integral = K8[I]; ; //derivative = K8[D] * (error8[0] - error8[1]); ; ; //calc new raw value ; new_duty = proportion + integral + derivative; 0A1D EC26 ldd 38,x 0A1F E324 addd 36,x 0A21 E322 addd 34,x 0A23 ED28 std 40,x 0A25 .dbline 1043 ; ; ; if (error8[0] < 0) { 0A25 FC0228 ldd _error8 0A28 2C0A bge L137 0A2A .dbline 1044 ; DIR_PORT &= 0x7F; 0A2A 18CE4000 ldy #0x4000 0A2E 181D0080 bclr 0,y,#0x80 0A32 .dbline 1045 ; } 0A32 2008 bra L138 0A34 L137: 0A34 .dbline 1047 ; else { ; DIR_PORT |= 0x80; 0A34 18CE4000 ldy #0x4000 0A38 181C0080 bset 0,y,#128 0A3C .dbline 1048 ; } 0A3C L138: 0A3C .dbline 1051 ; ; //take absolute value of new_duty ; if (new_duty < 0){ 0A3C EC28 ldd 40,x 0A3E 2C09 bge L139 0A40 EC28 ldd 40,x 0A42 830001 subd #1 0A45 43 coma 0A46 53 comb 0A47 ED28 std 40,x 0A49 .dbline 1057 ; asm("ldd %new_duty\n" ; "subd #1\n" ; "coma\n" ; "comb\n" ; "std %new_duty\n"); ; } 0A49 L139: 0A49 .dbline 1059 ; //range new_duty to max value ; if (new_duty > 236){ 0A49 EC28 ldd 40,x 0A4B 1A8300EC cpd #236 0A4F 2F05 ble L141 0A51 .dbline 1060 ; new_duty = 236; 0A51 CC00EC ldd #236 0A54 ED28 std 40,x 0A56 .dbline 1061 ; } 0A56 L141: 0A56 .dbline 1063 ; ; new_duty = pwm_vals[new_duty]; 0A56 EC28 ldd 40,x 0A58 05 lsld 0A59 C30000 addd #_pwm_vals 0A5C 188F xgdy 0A5E 18EC00 ldd 0,y 0A61 ED28 std 40,x 0A63 .dbline 1066 ; ; ; multvlve(new_duty, 0x08); 0A63 CC0008 ldd #8 0A66 37 pshb 0A67 36 psha 0A68 EC28 ldd 40,x 0A6A BD0069 jsr _multvlve 0A6D 1838 puly 0A6F .dbline 1068 ; ; } 0A6F L130: 0A6F .dbline 1073 ; ; ; ; ; handleSampleFlag = 0; 0A6F 7F01DD clr _handleSampleFlag 0A72 .dbline 1074 ; } 0A72 L6: 0A72 8F xgdx 0A73 C3002A addd #42 0A76 8F xgdx 0A77 35 txs 0A78 38 pulx 0A79 39 rts ; IX -> 0,x ; rMEM -> 2,x ; ?temp -> 4,x ; ?temp -> 6,x ; ?temp -> 8,x ; ?temp -> 10,x ; cylinderDes -> 12,x ; cylinderNumber -> 13,x ; counter -> 14,x 0A7A _decode_command:: 0A7A BD0000 jsr __enterb 0A7D 10 .byte 0x10 0A7E .dbfunc decode_command 0A7E .dbline 1086 ; ; ; /********************************************************/ ; /*decode_command() */ ; /*function to decode the commands recieved on the */ ; /*serial port */ ; /* */ ; /********************************************************/ ; ; void decode_command(void){ ; ; int counter = 0; 0A7E CC0000 ldd #0 0A81 ED0E std 14,x 0A83 .dbline 1087 ; char cylinderNumber = 0; 0A83 6F0D clr 13,x 0A85 .dbline 1088 ; char cylinderDes = 0; 0A85 6F0C clr 12,x 0A87 2007 bra L145 0A89 L144: 0A89 .dbline 1094 0A89 EC0E ldd 14,x 0A8B C30001 addd #1 0A8E ED0E std 14,x 0A90 .dbline 1095 0A90 L145: 0A90 .dbline 1093 ; ; ; //search through command string to find start char ; //in case of bogus data ; while(counter < COMMAND_LENGTH && Command[counter] != START_CHAR) 0A90 EC0E ldd 14,x 0A92 1A830007 cpd #7 0A96 2C0E bge L147 0A98 EC0E ldd 14,x 0A9A C30000 addd #_Command 0A9D 188F xgdy 0A9F 18E600 ldab 0,y 0AA2 C124 cmpb #36 0AA4 26E3 bne L144 0AA6 L147: 0AA6 .dbline 1097 ; {counter++; ; } ; ; if (counter == COMMAND_LENGTH){//bogus data so return 0AA6 EC0E ldd 14,x 0AA8 1A830007 cpd #7 0AAC 2606 bne L148 0AAE .dbline 1098 ; handleCommandFlag = 0; 0AAE 7F01DC clr _handleCommandFlag 0AB1 .dbline 1099 ; return; 0AB1 7E0B7D jmp L143 0AB4 L148: 0AB4 .dbline 1102 ; } ; ; switch (Command[counter + 1]){ 0AB4 EC0E ldd 14,x 0AB6 C30001 addd #_Command+1 0AB9 188F xgdy 0ABB 18E600 ldab 0,y 0ABE 4F clra 0ABF 5D tstb 0AC0 2A01 bpl X0 0AC2 43 coma 0AC3 X0: 0AC3 ED0A std 10,x 0AC5 1A830043 cpd #67 0AC9 2703 beq L154 0ACB 7E0B7A jmp L150 0ACE L154: 0ACE .dbline 1104 ; case 'C': //cylinder position value ; cylinderNumber = Command[counter + 2] & 0x0F; 0ACE EC0E ldd 14,x 0AD0 C30002 addd #_Command+2 0AD3 188F xgdy 0AD5 18E600 ldab 0,y 0AD8 C40F andb #15 0ADA E70D stab 13,x 0ADC .dbline 1105 ; Command[counter + 3] &= 0x0F; 0ADC EC0E ldd 14,x 0ADE C30003 addd #_Command+3 0AE1 ED08 std 8,x 0AE3 1AEE08 ldy 8,x 0AE6 183C pshy ; spill 0AE8 1AEE08 ldy 8,x 0AEB 1838 puly ; reload 0AED 181D00F0 bclr 0,y,#0xf0 0AF1 .dbline 1106 ; Command[counter + 4] &= 0x0F; 0AF1 EC0E ldd 14,x 0AF3 C30004 addd #_Command+4 0AF6 ED06 std 6,x 0AF8 1AEE06 ldy 6,x 0AFB 183C pshy ; spill 0AFD 1AEE06 ldy 6,x 0B00 1838 puly ; reload 0B02 181D00F0 bclr 0,y,#0xf0 0B06 .dbline 1107 ; Command[counter + 5] &= 0x0F; 0B06 EC0E ldd 14,x 0B08 C30005 addd #_Command+5 0B0B ED04 std 4,x 0B0D 1AEE04 ldy 4,x 0B10 183C pshy ; spill 0B12 1AEE04 ldy 4,x 0B15 1838 puly ; reload 0B17 181D00F0 bclr 0,y,#0xf0 0B1B .dbline 1108 ; cylinderDes = Command[counter + 3] * 100 + Command[counter + 4] * 10 + Command[counter + 5]; 0B1B EC0E ldd 14,x 0B1D C30004 addd #_Command+4 0B20 188F xgdy 0B22 18E600 ldab 0,y 0B25 4F clra 0B26 5D tstb 0B27 2A01 bpl X1 0B29 43 coma 0B2A X1: 0B2A 188F xgdy 0B2C CC000A ldd #10 0B2F BD0000 jsr __muli 0B32 ED02 std 2,x 0B34 EC0E ldd 14,x 0B36 C30003 addd #_Command+3 0B39 188F xgdy 0B3B 18E600 ldab 0,y 0B3E 4F clra 0B3F 5D tstb 0B40 2A01 bpl X2 0B42 43 coma 0B43 X2: 0B43 188F xgdy 0B45 CC0064 ldd #100 0B48 BD0000 jsr __muli 0B4B E302 addd 2,x 0B4D 37 pshb ; 0B4E 36 psha ; spill 0B4F EC0E ldd 14,x 0B51 C30005 addd #_Command+5 0B54 188F xgdy 0B56 18E600 ldab 0,y 0B59 4F clra 0B5A 5D tstb 0B5B 2A01 bpl X3 0B5D 43 coma 0B5E X3: 0B5E ED02 std 2,x 0B60 32 pula ; 0B61 33 pulb ; reload 0B62 E302 addd 2,x 0B64 E70C stab 12,x 0B66 E60D ldab 13,x 0B68 C001 subb #1 0B6A 58 lslb 0B6B A60C ldaa 12,x 0B6D FF02AA stx _x_preserve 0B70 CE01EE ldx #_cyl1_des 0B73 3A abx 0B74 08 inx 0B75 A700 staa 0,x 0B77 FE02AA ldx _x_preserve 0B7A .dbline 1121 ; //store new command into global variables ; asm("ldab %cylinderNumber\n" ; "subb #1\n" ; "lslb\n" ; "ldaa %cylinderDes\n" ; "stx _x_preserve\n" ; "ldx #_cyl1_des\n" ; "abx\n" ; "inx\n" ; "staa 0,x\n" ; "ldx _x_preserve"); ; ; break ; 0B7A L150: 0B7A L151: 0B7A .dbline 1127 ; ; //put next command case here ; ; }//ends switch statement ; ; handleCommandFlag = 0; 0B7A 7F01DC clr _handleCommandFlag 0B7D .dbline 1128 ; } 0B7D L143: 0B7D 8F xgdx 0B7E C30010 addd #16 0B81 8F xgdx 0B82 35 txs 0B83 38 pulx 0B84 39 rts ; IX -> 0,x ; byte -> 5,x 0B85 _send_message:: 0B85 37 pshb 0B86 36 psha 0B87 3C pshx 0B88 3C pshx 0B89 30 tsx 0B8A EF00 stx 0,x 0B8C .dbfunc send_message 0B8C .dbline 1139 ; ; /********************************************************/ ; /*send_message(byte) */ ; /*function to send a byte out the serial port */ ; /* */ ; /* */ ; /********************************************************/ ; ; void send_message(char byte) ; { ; Message = byte; 0B8C E605 ldab 5,x 0B8E F701DA stab _Message 0B91 .dbline 1140 ; SCCR2 |= 0x80; 0B91 18CE102D ldy #0x102d 0B95 181C0080 bset 0,y,#128 0B99 .dbline 1141 ; } 0B99 L162: 0B99 08 inx 0B9A 08 inx 0B9B 35 txs 0B9C 38 pulx 0B9D 1838 puly 0B9F 39 rts ; IX -> 0,x ; ?temp -> 3,x 0BA0 _SCI_isr:: 0BA0 BD0000 jsr __enterb 0BA3 04 .byte 0x4 0BA4 .dbfunc SCI_isr 0BA4 .dbline 1154 ; ; /********************************************************/ ; /*ISRs */ ; /* */ ; /* */ ; /* */ ; /********************************************************/ ; ; /* Captures incoming messages of CommandLength bytes */ ; /* Sends one byte messages then disables TIE */ ; /* SCI isr */ ; void SCI_isr() { ; if(SCSR & 0X20) { 0BA4 18CE102E ldy #0x102e 0BA8 181F00203F brclr 0,y,#32,L164 0BAD .dbline 1155 ; Command[CommandPos] = SCDR; 0BAD F601DB ldab _CommandPos 0BB0 4F clra 0BB1 5D tstb 0BB2 2A01 bpl X4 0BB4 43 coma 0BB5 X4: 0BB5 C30000 addd #_Command 0BB8 188F xgdy ; vol 0BBA F6102F ldab 0x102f 0BBD 18E700 stab 0,y 0BC0 .dbline 1156 ; send_message(SCDR); // Include to echo characters ; vol 0BC0 F6102F ldab 0x102f 0BC3 4F clra 0BC4 5D tstb 0BC5 2A01 bpl X5 0BC7 43 coma 0BC8 X5: 0BC8 BD0B85 jsr _send_message 0BCB .dbline 1157 ; if (++CommandPos == COMMAND_LENGTH) { 0BCB F601DB ldab _CommandPos 0BCE CB01 addb #1 0BD0 E703 stab 3,x 0BD2 F701DB stab _CommandPos 0BD5 E603 ldab 3,x 0BD7 C107 cmpb #7 0BD9 2603 bne L166 0BDB .dbline 1158 ; CommandPos = 0; 0BDB 7F01DB clr _CommandPos 0BDE .dbline 1159 ; } 0BDE L166: 0BDE .dbline 1160 ; if (SCDR == END_CHAR) { ; vol 0BDE F6102F ldab 0x102f 0BE1 C123 cmpb #35 0BE3 261E bne L165 0BE5 .dbline 1161 ; handleCommandFlag = 1; 0BE5 C601 ldab #1 0BE7 F701DC stab _handleCommandFlag 0BEA .dbline 1163 ; } ; } 0BEA 2017 bra L165 0BEC L164: 0BEC .dbline 1164 ; else if(SCSR & 0X80) { 0BEC 18CE102E ldy #0x102e 0BF0 181F00800E brclr 0,y,#128,L170 0BF5 .dbline 1165 ; SCDR = Message; 0BF5 F601DA ldab _Message 0BF8 F7102F stab 0x102f 0BFB .dbline 1166 ; SCCR2 &= 0X7F; 0BFB 18CE102D ldy #0x102d 0BFF 181D0080 bclr 0,y,#0x80 0C03 .dbline 1167 ; } 0C03 L170: 0C03 L165: 0C03 .dbline 1169 ; ; } 0C03 L163: 0C03 08 inx 0C04 08 inx 0C05 08 inx 0C06 08 inx 0C07 35 txs 0C08 38 pulx 0C09 3B rti ; IX -> 0,x 0C0A _TOC5_isr:: 0C0A .dbfunc TOC5_isr 0C0A .dbline 1175 ; ; ; /*TOC5_isr- controls motors 7 and 8*/ ; void TOC5_isr() ; { ; TFLG1 = 0x08; //clear interrupt flag 0C0A C608 ldab #8 0C0C F71023 stab 0x1023 0C0F 18FE027E ldy _oc5_reg 0C13 B602AC ldaa _mot_spd 0C16 843F anda #$3f 0C18 18AA00 oraa 0,y 0C1B B702AC staa _mot_spd 0C1E FE02AD ldx _pwm_port 0C21 A700 staa 0,x 0C23 1808 iny 0C25 CE101E ldx #$101E 0C28 18EC00 ldd 0,y 0C2B E300 addd 0,x 0C2D ED00 std 0,x 0C2F 188C0287 cpy #_oc5_3vl 0C33 2604 bne oc5_rti 0C35 18CE027E ldy #_oc5_reg 0C39 oc5_rti: 0C39 1808 iny 0C3B 1808 iny 0C3D 18FF027E sty _oc5_reg 0C41 .dbline 1195 ; asm("ldy _oc5_reg\n" ; "ldaa _mot_spd\n" ; "anda #$3f\n" ; "oraa 0,y\n" ; "staa _mot_spd\n" ; "ldx _pwm_port\n" ; "staa 0,x\n" ; "iny\n" ; "ldx #$101E\n" ; "ldd 0,y\n" ; "addd 0,x\n" ; "std 0,x\n" ; "cpy #_oc5_3vl\n" ; "bne oc5_rti\n" ; "ldy #_oc5_reg\n" ; "oc5_rti:\n" ; "iny\n" ; "iny\n" ; "sty _oc5_reg"); ; } 0C41 L172: 0C41 3B rti ; IX -> 0,x 0C42 _TOC4_isr:: 0C42 .dbfunc TOC4_isr 0C42 .dbline 1200 ; ; /*TOC4_isr - controls motors 5 and 6*/ ; void TOC4_isr() ; { ; TFLG1 = 0X10; //clear interrupt flag 0C42 C610 ldab #16 0C44 F71023 stab 0x1023 0C47 18FE0289 ldy _oc4_reg 0C4B B602AC ldaa _mot_spd 0C4E 84CF anda #$cf 0C50 18AA00 oraa 0,y 0C53 B702AC staa _mot_spd 0C56 FE02AD ldx _pwm_port 0C59 A700 staa 0,x 0C5B 1808 iny 0C5D CE101C ldx #$101C 0C60 18EC00 ldd 0,y 0C63 E300 addd 0,x 0C65 ED00 std 0,x 0C67 188C0292 cpy #_oc4_3vl 0C6B 2604 bne oc4_rti 0C6D 18CE0289 ldy #_oc4_reg 0C71 oc4_rti: 0C71 1808 iny 0C73 1808 iny 0C75 18FF0289 sty _oc4_reg 0C79 .dbline 1220 ; asm("ldy _oc4_reg\n" ; "ldaa _mot_spd\n" ; "anda #$cf\n" ; "oraa 0,y\n" ; "staa _mot_spd\n" ; "ldx _pwm_port\n" ; "staa 0,x\n" ; "iny\n" ; "ldx #$101C\n" ; "ldd 0,y\n" ; "addd 0,x\n" ; "std 0,x\n" ; "cpy #_oc4_3vl\n" ; "bne oc4_rti\n" ; "ldy #_oc4_reg\n" ; "oc4_rti:\n" ; "iny\n" ; "iny\n" ; "sty _oc4_reg"); ; } 0C79 L173: 0C79 3B rti ; IX -> 0,x 0C7A _TOC3_isr:: 0C7A .dbfunc TOC3_isr 0C7A .dbline 1226 ; ; ; /*TOC3_isr - controls motors 3 and 4*/ ; void TOC3_isr() ; { ; TFLG1 = 0x20; //clear interrupt flag 0C7A C620 ldab #32 0C7C F71023 stab 0x1023 0C7F 18FE0294 ldy _oc3_reg 0C83 B602AC ldaa _mot_spd 0C86 84F3 anda #$f3 0C88 18AA00 oraa 0,y 0C8B B702AC staa _mot_spd 0C8E FE02AD ldx _pwm_port 0C91 A700 staa 0,x 0C93 1808 iny 0C95 CE101A ldx #$101A 0C98 18EC00 ldd 0,y 0C9B E300 addd 0,x 0C9D ED00 std 0,x 0C9F 188C029D cpy #_oc3_3vl 0CA3 2604 bne oc3_rti 0CA5 18CE0294 ldy #_oc3_reg 0CA9 oc3_rti: 0CA9 1808 iny 0CAB 1808 iny 0CAD 18FF0294 sty _oc3_reg 0CB1 .dbline 1246 ; asm("ldy _oc3_reg\n" ; "ldaa _mot_spd\n" ; "anda #$f3\n" ; "oraa 0,y\n" ; "staa _mot_spd\n" ; "ldx _pwm_port\n" ; "staa 0,x\n" ; "iny\n" ; "ldx #$101A\n" ; "ldd 0,y\n" ; "addd 0,x\n" ; "std 0,x\n" ; "cpy #_oc3_3vl\n" ; "bne oc3_rti\n" ; "ldy #_oc3_reg\n" ; "oc3_rti:\n" ; "iny\n" ; "iny\n" ; "sty _oc3_reg"); ; } 0CB1 L174: 0CB1 3B rti ; IX -> 0,x 0CB2 _TOC2_isr:: 0CB2 .dbfunc TOC2_isr 0CB2 .dbline 1253 ; ; ; ; /*TOC2_isr - controls motors 1 and 2*/ ; void TOC2_isr() ; { ; TFLG1 = 0x40; //clear interrupt flag 0CB2 C640 ldab #64 0CB4 F71023 stab 0x1023 0CB7 18FE029F ldy _oc2_reg 0CBB B602AC ldaa _mot_spd 0CBE 84FC anda #$fc 0CC0 18AA00 oraa 0,y 0CC3 B702AC staa _mot_spd 0CC6 FE02AD ldx _pwm_port 0CC9 A700 staa 0,x 0CCB 1808 iny 0CCD CE1018 ldx #$1018 0CD0 18EC00 ldd 0,y 0CD3 E300 addd 0,x 0CD5 ED00 std 0,x 0CD7 188C02A8 cpy #_oc2_3vl 0CDB 2604 bne oc2_rti 0CDD 18CE029F ldy #_oc2_reg 0CE1 oc2_rti: 0CE1 1808 iny 0CE3 1808 iny 0CE5 18FF029F sty _oc2_reg 0CE9 .dbline 1274 ; ; asm("ldy _oc2_reg\n" ; "ldaa _mot_spd\n" ; "anda #$fc\n" ; "oraa 0,y\n" ; "staa _mot_spd\n" ; "ldx _pwm_port\n" ; "staa 0,x\n" ; "iny\n" ; "ldx #$1018\n" ; "ldd 0,y\n" ; "addd 0,x\n" ; "std 0,x\n" ; "cpy #_oc2_3vl\n" ; "bne oc2_rti\n" ; "ldy #_oc2_reg\n" ; "oc2_rti:\n" ; "iny\n" ; "iny\n" ; "sty _oc2_reg"); ; } 0CE9 L175: 0CE9 3B rti ; IX -> 0,x 0CEA _RTI_isr:: 0CEA .dbfunc RTI_isr 0CEA .dbline 1280 ; ; /*RTI_isr - samples analog values on every third interrupt*/ ; /*sets a flag to call PID controller*/ ; void RTI_isr() ; { ; TFLG2 = 0x40; //clear flag 0CEA C640 ldab #64 0CEC F71025 stab 0x1025 0CEF .dbline 1281 ; if (RTIcount < 2) 0CEF F602B1 ldab _RTIcount 0CF2 C102 cmpb #2 0CF4 2C05 bge L177 0CF6 .dbline 1283 ; { ; RTIcount++; 0CF6 7C02B1 inc _RTIcount 0CF9 .dbline 1284 ; return; 0CF9 204C bra L176 0CFB L177: 0CFB .dbline 1287 ; } ; ; RTIcount = 0; 0CFB 7F02B1 clr _RTIcount 0CFE .dbline 1290 ; ; //run sample of all 8 cylinders ; ADCTL = 0x90 //CCF=1 SCAN=0 MULT=1 CD-CA=0000 0CFE C690 ldab #144 0D00 F71030 stab 0x1030 0D03 8615 ldaa #21 0D05 loop: 0D05 4A deca 0D06 26FD bne loop 0D08 B61031 ldaa $1031 0D0B B701DF staa _cyl1_pstn + 1 0D0E B61032 ldaa $1032 0D11 B701E1 staa _cyl2_pstn + 1 0D14 B61033 ldaa $1033 0D17 B701E3 staa _cyl3_pstn + 1 0D1A B61034 ldaa $1034 0D1D B701E5 staa _cyl4_pstn + 1 0D20 .dbline 1316 ; //wait for complete samples ; asm("ldaa #21\n" ; "loop:\n" ; "deca\n" ; "bne loop"); ; ; //now load in the analog values ; //assembly routines to change char to int ; ; //cyl1_pstn = ADR1; ; asm("ldaa $1031\n" ; "staa _cyl1_pstn + 1\n"); ; ; //cyl2_pstn = ADR2; ; asm("ldaa $1032\n" ; "staa _cyl2_pstn + 1\n"); ; ; //cyl3_pstn = ADR3; ; asm("ldaa $1033\n" ; "staa _cyl3_pstn + 1\n"); ; ; //cyl4_pstn = ADR4; ; asm("ldaa $1034\n" ; "staa _cyl4_pstn + 1\n"); ; ; ADCTL = 0x94 //CCF=1 SCAN=0 MULT=1 CD-CA=0100 0D20 C694 ldab #148 0D22 F71030 stab 0x1030 0D25 8615 ldaa #21 0D27 loop2: 0D27 4A deca 0D28 26FD bne loop2 0D2A B61031 ldaa $1031 0D2D B701E7 staa _cyl5_pstn + 1 0D30 B61032 ldaa $1032 0D33 B701E9 staa _cyl6_pstn + 1 0D36 B61033 ldaa $1033 0D39 B701EB staa _cyl7_pstn + 1 0D3C B61034 ldaa $1034 0D3F B701ED staa _cyl8_pstn + 1 0D42 .dbline 1342 ; //wait for complete samples ; asm("ldaa #21\n" ; "loop2:\n" ; "deca\n" ; "bne loop2"); ; ; //cyl5_pstn = ADR1; ; asm("ldaa $1031\n" ; "staa _cyl5_pstn + 1\n"); ; ; //cyl6_pstn = ADR2; ; asm("ldaa $1032\n" ; "staa _cyl6_pstn + 1\n"); ; ; //cyl7_pstn = ADR3; ; asm("ldaa $1033\n" ; "staa _cyl7_pstn + 1\n"); ; ; //cyl8_pstn = ADR4; ; asm("ldaa $1034\n" ; "staa _cyl8_pstn + 1\n"); ; ; //finished with sampling ; ; ; handleSampleFlag = 1; //set flag to run pid routine 0D42 C601 ldab #1 0D44 F701DD stab _handleSampleFlag 0D47 .dbline 1344 ; ; } 0D47 L176: 0D47 3B rti ; IX -> 0,x ; count -> 2,x 0D48 _main:: 0D48 BD0000 jsr __enterb 0D4B 04 .byte 0x4 0D4C .dbfunc main 0D4C .dbline 1356 ; ; /********************************************************/ ; /*Begin main function */ ; /* */ ; /* */ ; /* */ ; /********************************************************/ ; ; void main(void) ; { ; ; int count = 1700; 0D4C CC06A4 ldd #1700 0D4F ED02 std 2,x 0D51 .dbline 1360 ; /*this must be here to initalize the pointers and ; leave them in the correct momory space*/ ; ; oc2_reg = &oc2_1ot; 0D51 CC02A1 ldd #_oc2_1ot 0D54 FD029F std _oc2_reg 0D57 .dbline 1361 ; oc3_reg = &oc3_1ot; 0D57 CC0296 ldd #_oc3_1ot 0D5A FD0294 std _oc3_reg 0D5D .dbline 1362 ; oc4_reg = &oc4_1ot; 0D5D CC028B ldd #_oc4_1ot 0D60 FD0289 std _oc4_reg 0D63 .dbline 1363 ; oc5_reg = &oc5_1ot; 0D63 CC0280 ldd #_oc5_1ot 0D66 FD027E std _oc5_reg 0D69 .dbline 1365 ; ; init_analog(); 0D69 BD0000 jsr _init_analog 0D6C .dbline 1366 ; init_SCI(); 0D6C BD0000 jsr _init_SCI 0D6F .dbline 1367 ; init_pulse(); //set up interrupts for oc2-5 0D6F BD001C jsr _init_pulse 0D72 .dbline 1368 ; init_RTI(); 0D72 BD0058 jsr _init_RTI 0D75 2010 bra L181 0D77 L180: 0D77 .dbline 1374 ; ; //printf("\n\n starting\n"); ; //multvlve(20000, 0x01); ; ; while(1){ //main loop ; if(handleSampleFlag){ 0D77 7D01DD tst _handleSampleFlag 0D7A 2703 beq L183 0D7C .dbline 1375 ; pid_controller(); 0D7C BD0467 jsr _pid_controller 0D7F .dbline 1376 ; }//ends if 0D7F L183: 0D7F .dbline 1378 ; ; if(handleCommandFlag){ 0D7F 7D01DC tst _handleCommandFlag 0D82 2703 beq L185 0D84 .dbline 1379 ; decode_command(); 0D84 BD0A7A jsr _decode_command 0D87 .dbline 1380 ; }//ends if 0D87 L185: 0D87 .dbline 1382 ; ; }//ends main while 0D87 L181: 0D87 20EE bra L180 0D89 L179: 0D89 08 inx 0D8A 08 inx 0D8B 08 inx 0D8C 08 inx 0D8D 35 txs 0D8E 38 pulx 0D8F 39 rts .area memory(abs) .org 0xffd6 FFD6 _interrupt_vectors:: FFD6 0BA0 .word _SCI_isr FFD8 FFFF .word 65535 FFDA FFFF .word 65535 FFDC FFFF .word 65535 FFDE FFFF .word 65535 FFE0 0C0A .word _TOC5_isr FFE2 0C42 .word _TOC4_isr FFE4 0C7A .word _TOC3_isr FFE6 0CB2 .word _TOC2_isr FFE8 FFFF .word 65535 FFEA FFFF .word 65535 FFEC FFFF .word 65535 FFEE FFFF .word 65535 FFF0 0CEA .word _RTI_isr FFF2 FFFF .word 65535 FFF4 FFFF .word 65535 FFF6 FFFF .word 65535 FFF8 FFFF .word 65535 FFFA FFFF .word 65535 FFFC FFFF .word 65535 FFFE 0000 .word __start .area data .area bss 0000 _Command:: 0000 .blkb 14