main.lst - generated by MGTEK Assembler ASM12 V1.16 Build 101 for WIN32 (x86) - Thu Apr 18 02:50:05 2002 1: * Filename : MAIN.ASM 2: * Programmer : Michael Hattermann 3: * Date : February 22, 2002 4: * Version : 1.0 5: * Description : This file contains the main routine to 6: * control the rest of the robot. It 7: * includes all the other control files. 8: * The followong functions are available: 9: * 10: * MAIN - start of program, inits/uses systems 11: * MAINOUT - end of program, kills systems 12: * 13: 14: #include "hc12.asm" 1: * Filename : HC12.H 2: * Programmer : Michael Hattermann 3: * Date : February 21, 2002 4: * Version : 1.0 5: * Description : This file contains equates for the HC12 6: * processor 7: * 8: * 9: ************************************************************ 10: * Register Equates 11: ************************************************************ 12: * 13: #ifndef __HC12_ASM_ 14: =00000001 #define __HC12_ASM_ 1 15: 16: =00000000 IO_BASE EQU 0 17: 18: =00000000 PORTA EQU (IO_BASE + $00) 19: =00000001 PORTB EQU (IO_BASE + $01) 20: =00000002 DDRA EQU (IO_BASE + $02) 21: =00000003 DDRB EQU (IO_BASE + $03) 22: =00000008 PORTE EQU (IO_BASE + $08) 23: =00000009 DDRE EQU (IO_BASE + $09) 24: =0000000A PEAR EQU (IO_BASE + $0A) 25: =0000000B MODE EQU (IO_BASE + $0B) 26: =0000000C PUCR EQU (IO_BASE + $0C) 27: =0000000D RDRIV EQU (IO_BASE + $0D) 28: =00000010 INITRM EQU (IO_BASE + $10) 29: =00000011 INITRG EQU (IO_BASE + $11) 30: =00000012 INITEE EQU (IO_BASE + $12) 31: =00000013 MISC EQU (IO_BASE + $13) 32: =00000014 RTICTL EQU (IO_BASE + $14) 33: =00000015 RTIFLG EQU (IO_BASE + $15) 34: =00000016 COPCTL EQU (IO_BASE + $16) 35: =00000017 COPRST EQU (IO_BASE + $17) 36: =0000001E INTCR EQU (IO_BASE + $1E) 37: =0000001F HPRIO EQU (IO_BASE + $1F) 38: =00000020 BRKCT0 EQU (IO_BASE + $20) 39: =00000021 BRKCT1 EQU (IO_BASE + $21) 40: =00000022 BRKAH EQU (IO_BASE + $22) 41: =00000023 BRKAL EQU (IO_BASE + $23) 42: =00000024 BRKDH EQU (IO_BASE + $24) 43: =00000025 BRKDL EQU (IO_BASE + $25) 44: =00000040 PWCLK EQU (IO_BASE + $40) 45: =00000041 PWPOL EQU (IO_BASE + $41) 46: =00000042 PWEN EQU (IO_BASE + $42) 47: =00000043 PWPRES EQU (IO_BASE + $43) 48: =00000044 PWSCAL0 EQU (IO_BASE + $44) 49: =00000045 PWSCNT0 EQU (IO_BASE + $45) 50: =00000046 PWSCAL1 EQU (IO_BASE + $46) 51: =00000047 PWSCNT1 EQU (IO_BASE + $47) 52: =00000048 PWCNT0 EQU (IO_BASE + $48) 53: =00000049 PWCNT1 EQU (IO_BASE + $49) 54: =0000004A PWCNT2 EQU (IO_BASE + $4A) 55: =0000004B PWCNT3 EQU (IO_BASE + $4B) 56: =0000004C PWPER0 EQU (IO_BASE + $4C) 57: =0000004D PWPER1 EQU (IO_BASE + $4D) 58: =0000004E PWPER2 EQU (IO_BASE + $4E) 59: =0000004F PWPER3 EQU (IO_BASE + $4F) 60: =00000050 PWDTY0 EQU (IO_BASE + $50) 61: =00000051 PWDTY1 EQU (IO_BASE + $51) 62: =00000052 PWDTY2 EQU (IO_BASE + $52) 63: =00000053 PWDTY3 EQU (IO_BASE + $53) 64: =00000054 PWCTL EQU (IO_BASE + $54) 65: =00000055 PWTST EQU (IO_BASE + $55) 66: =00000056 PORTP EQU (IO_BASE + $56) 67: =00000057 DDRP EQU (IO_BASE + $57) 68: =00000060 ATDCTL0 EQU (IO_BASE + $60) 69: =00000061 ATDCTL1 EQU (IO_BASE + $61) 70: =00000062 ATDCTL2 EQU (IO_BASE + $62) 71: =00000063 ATDCTL3 EQU (IO_BASE + $63) 72: =00000064 ATDCTL4 EQU (IO_BASE + $64) 73: =00000065 ATDCTL5 EQU (IO_BASE + $65) 74: =00000066 ATDSTAT EQU (IO_BASE + $66) 75: =00000066 ATDSTATH EQU (IO_BASE + $66) 76: =00000067 ATDSTATL EQU (IO_BASE + $67) 77: =00000068 ATDTST EQU (IO_BASE + $68) 78: =00000068 ATDTSTH EQU (IO_BASE + $68) 79: =00000069 ATDTSTL EQU (IO_BASE + $69) 80: =0000006F PORTAD EQU (IO_BASE + $6F) 81: =00000070 ADR0 EQU (IO_BASE + $70) 82: =00000070 ADR0H EQU (IO_BASE + $70) 83: =00000071 ADR0L EQU (IO_BASE + $71) 84: =00000072 ADR1 EQU (IO_BASE + $72) 85: =00000072 ADR1H EQU (IO_BASE + $72) 86: =00000073 ADR1L EQU (IO_BASE + $73) 87: =00000074 ADR2 EQU (IO_BASE + $74) 88: =00000074 ADR2H EQU (IO_BASE + $74) 89: =00000075 ADR2L EQU (IO_BASE + $75) 90: =00000076 ADR3 EQU (IO_BASE + $76) 91: =00000076 ADR3H EQU (IO_BASE + $76) 92: =00000077 ADR3L EQU (IO_BASE + $77) 93: =00000078 ADR4 EQU (IO_BASE + $78) 94: =00000078 ADR4H EQU (IO_BASE + $78) 95: =00000079 ADR4L EQU (IO_BASE + $79) 96: =0000007A ADR5 EQU (IO_BASE + $7A) 97: =0000007A ADR5H EQU (IO_BASE + $7A) 98: =0000007B ADR5L EQU (IO_BASE + $7B) 99: =0000007C ADR6 EQU (IO_BASE + $7C) 100: =0000007C ADR6H EQU (IO_BASE + $7C) 101: =0000007D ADR6L EQU (IO_BASE + $7D) 102: =0000007E ADR7 EQU (IO_BASE + $7E) 103: =0000007E ADR7H EQU (IO_BASE + $7E) 104: =0000007F ADR7L EQU (IO_BASE + $7F) 105: =00000080 TIOS EQU (IO_BASE + $80) 106: =00000081 CFORC EQU (IO_BASE + $81) 107: =00000082 OC7M EQU (IO_BASE + $82) 108: =00000083 OC7D EQU (IO_BASE + $83) 109: =00000084 TCNT EQU (IO_BASE + $84) 110: =00000084 TCNTH EQU (IO_BASE + $84) 111: =00000085 TCNTL EQU (IO_BASE + $85) 112: =00000086 TSCR EQU (IO_BASE + $86) 113: =00000088 TCTL1 EQU (IO_BASE + $88) 114: =00000089 TCTL2 EQU (IO_BASE + $89) 115: =0000008A TCTL3 EQU (IO_BASE + $8A) 116: =0000008B TCTL4 EQU (IO_BASE + $8B) 117: =0000008C TMSK1 EQU (IO_BASE + $8C) 118: =0000008D TMSK2 EQU (IO_BASE + $8D) 119: =0000008E TFLG1 EQU (IO_BASE + $8E) 120: =0000008F TFLG2 EQU (IO_BASE + $8F) 121: =00000090 TC0 EQU (IO_BASE + $90) 122: =00000090 TC0H EQU (IO_BASE + $90) 123: =00000091 TC0L EQU (IO_BASE + $91) 124: =00000092 TC1 EQU (IO_BASE + $92) 125: =00000092 TC1H EQU (IO_BASE + $92) 126: =00000093 TC1L EQU (IO_BASE + $93) 127: =00000094 TC2 EQU (IO_BASE + $94) 128: =00000094 TC2H EQU (IO_BASE + $94) 129: =00000095 TC2L EQU (IO_BASE + $95) 130: =00000096 TC3 EQU (IO_BASE + $96) 131: =00000096 TC3H EQU (IO_BASE + $96) 132: =00000097 TC3L EQU (IO_BASE + $97) 133: =00000098 TC4 EQU (IO_BASE + $98) 134: =00000098 TC4H EQU (IO_BASE + $98) 135: =00000099 TC4L EQU (IO_BASE + $99) 136: =0000009A TC5 EQU (IO_BASE + $9A) 137: =0000009A TC5H EQU (IO_BASE + $9A) 138: =0000009B TC5L EQU (IO_BASE + $9B) 139: =0000009C TC6 EQU (IO_BASE + $9C) 140: =0000009C TC6H EQU (IO_BASE + $9C) 141: =0000009D TC6L EQU (IO_BASE + $9D) 142: =0000009E TC7 EQU (IO_BASE + $9E) 143: =0000009E TC7H EQU (IO_BASE + $9E) 144: =0000009F TC7L EQU (IO_BASE + $9F) 145: =000000A0 PACTL EQU (IO_BASE + $A0) 146: =000000A1 PAFLG EQU (IO_BASE + $A1) 147: =000000A2 PACN3 EQU (IO_BASE + $A2) 148: =000000A3 PACN2 EQU (IO_BASE + $A3) 149: =000000A4 PACN1 EQU (IO_BASE + $A4) 150: =000000A5 PACN0 EQU (IO_BASE + $A5) 151: =000000A6 MCCTL EQU (IO_BASE + $A6) 152: =000000A7 MCFLG EQU (IO_BASE + $A7) 153: =000000A8 ICPACR EQU (IO_BASE + $A8) 154: =000000A9 DLYCT EQU (IO_BASE + $A9) 155: =000000AA ICOVW EQU (IO_BASE + $AA) 156: =000000AB ICSYS EQU (IO_BASE + $AB) 157: =000000AD TIMTST EQU (IO_BASE + $AD) 158: =000000AE PORTT EQU (IO_BASE + $AE) 159: =000000AF DDRT EQU (IO_BASE + $AF) 160: =000000B0 PBCTL EQU (IO_BASE + $B0) 161: =000000B1 PBFLG EQU (IO_BASE + $B1) 162: =000000B2 PA3H EQU (IO_BASE + $B2) 163: =000000B3 PA2H EQU (IO_BASE + $B3) 164: =000000B4 PA1H EQU (IO_BASE + $B4) 165: =000000B5 PA0H EQU (IO_BASE + $B5) 166: =000000B6 MCCNT EQU (IO_BASE + $B6) 167: =000000B6 MCCNTH EQU (IO_BASE + $B6) 168: =000000B7 MCCNTL EQU (IO_BASE + $B7) 169: =000000C0 SC0BD EQU (IO_BASE + $C0) 170: =000000C0 SC0BDH EQU (IO_BASE + $C0) 171: =000000C1 SC0BDL EQU (IO_BASE + $C1) 172: =000000C2 SC0CR1 EQU (IO_BASE + $C2) 173: =000000C3 SC0CR2 EQU (IO_BASE + $C3) 174: =000000C4 SC0SR1 EQU (IO_BASE + $C4) 175: =000000C5 SC0SR2 EQU (IO_BASE + $C5) 176: =000000C6 SC0DRH EQU (IO_BASE + $C6) 177: =000000C7 SC0DRL EQU (IO_BASE + $C7) 178: =000000D0 SP0CR1 EQU (IO_BASE + $D0) 179: =000000D1 SP0CR2 EQU (IO_BASE + $D1) 180: =000000D2 SP0BR EQU (IO_BASE + $D2) 181: =000000D3 SP0SR EQU (IO_BASE + $D3) 182: =000000D5 SP0DR EQU (IO_BASE + $D5) 183: =000000D6 PORTS EQU (IO_BASE + $D6) 184: =000000D7 DDRS EQU (IO_BASE + $D7) 185: =000000DB PURDS EQU (IO_BASE + $DB) 186: =000000E0 SLOW EQU (IO_BASE + $E0) 187: =000000F0 EEMCR EQU (IO_BASE + $F0) 188: =000000F1 EEPROT EQU (IO_BASE + $F1) 189: =000000F2 EETST EQU (IO_BASE + $F2) 190: =000000F3 EEPROG EQU (IO_BASE + $F3) 191: =000000F4 FEELCK EQU (IO_BASE + $F4) 192: =000000F5 FEEMCR EQU (IO_BASE + $F5) 193: =000000F6 FEETST EQU (IO_BASE + $F6) 194: =000000F7 FEECTL EQU (IO_BASE + $F7) 195: =000000F8 BCR1 EQU (IO_BASE + $F8) 196: =000000F9 BSVR EQU (IO_BASE + $F9) 197: =000000FA BCR2 EQU (IO_BASE + $FA) 198: =000000FB BDR EQU (IO_BASE + $FB) 199: =000000FC BARD EQU (IO_BASE + $FC) 200: =000000FD DLCSCR EQU (IO_BASE + $FD) 201: =000000FE PORTDLC EQU (IO_BASE + $FE) 202: =000000FF DDRDLC EQU (IO_BASE + $FF) 203: * 204: ************************************************************ 205: * Interrupt vector addresses 206: ************************************************************ 207: * 208: =0000FFCC MOD_UNDER_IVECT EQU $FFCC 209: =0000FFCA PACB_OVER_IVECT EQU $FFCA 210: =0000FFD0 BDLC_IVECT EQU $FFD0 211: =0000FFD2 ATD_IVECT EQU $FFD2 212: =0000FFD4 RES4_IVECT EQU $FFD4 213: =0000FFD6 SCI0_IVECT EQU $FFD6 214: =0000FFD8 SPI_TC_IVECT EQU $FFD8 215: =0000FFDA PAC_IN_EDGE_IVECT EQU $FFDA 216: =0000FFDC PAC_OVER_IVECT EQU $FFDC 217: =0000FFDE TMR_OVER_IVECT EQU $FFDE 218: =0000FFE0 T7_IVECT EQU $FFE0 219: =0000FFE2 T6_IVECT EQU $FFE2 220: =0000FFE4 T5_IVECT EQU $FFE4 221: =0000FFE6 T4_IVECT EQU $FFE6 222: =0000FFE8 T3_IVECT EQU $FFE8 223: =0000FFEA T2_IVECT EQU $FFEA 224: =0000FFEC T1_IVECT EQU $FFEC 225: =0000FFEE T0_IVECT EQU $FFEE 226: =0000FFF0 RTI_IVECT EQU $FFF0 227: =0000FFF0 IRQ_IVECT EQU $FFF0 228: =0000FFF0 XIRQ_IVECT EQU $FFF0 229: =0000FFF0 SWI_IVECT EQU $FFF0 230: =0000FFF0 TRAP_IVECT EQU $FFF0 231: =0000FFF0 COP_FAIL_IVECT EQU $FFF0 232: =0000FFF0 COP_CLK_FAIL_IVECT EQU $FFF0 233: =0000FFF0 RESET_IVECT EQU $FFF0 234: * 235: ************************************************************ 236: * Pseudointerrupt vector addresses 237: ************************************************************ 238: * 239: =00000800 MOD_UNDER_PVECT EQU $0800 240: =00000803 PACB_OVER_PVECT EQU $0803 241: =00000806 BDLC_PVECT EQU $0806 242: =00000809 ATD_PVECT EQU $0809 243: =0000080C SCI0_PVECT EQU $080C 244: =0000080F SPI_TC_PVECT EQU $080F 245: =00000812 PAC_IN_EDGE_PVECT EQU $0812 246: =00000815 PAC_OVER_PVECT EQU $0815 247: =00000818 TMR_OVER_PVECT EQU $0818 248: =0000081B T7_PVECT EQU $081B 249: =0000081E T6_PVECT EQU $081E 250: =00000821 T5_PVECT EQU $0821 251: =00000824 T4_PVECT EQU $0824 252: =00000827 T3_PVECT EQU $0827 253: =0000082A T2_PVECT EQU $082A 254: =0000082D T1_PVECT EQU $082D 255: =00000830 T0_PVECT EQU $0830 256: =00000833 RTI_PVECT EQU $0833 257: =00000836 IRQ_PVECT EQU $0836 258: =00000839 XIRQ_PVECT EQU $0839 259: =0000083C SWI_PVECT EQU $083C 260: =0000083F TRAP_PVECT EQU $083F 261: =00000842 COP_FAIL_PVECT EQU $0842 262: =00000845 COP_CLK_FAIL_PVECT EQU $0845 263: =00000848 RESET_PVECT EQU $0848 264: =0000084B USERPROG_PVECT EQU $084B 265: * 266: ************************************************************ 267: * Bit Equates 268: ************************************************************ 269: * 270: =00000001 BIT0 EQU %00000001 ; bit 0 271: =000000FE INV0 EQU %11111110 ; inverse of bit 0 272: =00000002 BIT1 EQU %00000010 ; bit 1 273: =000000FD INV1 EQU %11111101 ; inverse of bit 1 274: =00000004 BIT2 EQU %00000100 ; bit 2 275: =000000FB INV2 EQU %11111011 ; inverse of bit 2 276: =00000008 BIT3 EQU %00001000 ; bit 3 277: =000000F7 INV3 EQU %11110111 ; inverse of bit 3 278: =00000010 BIT4 EQU %00010000 ; bit 4 279: =000000EF INV4 EQU %11101111 ; inverse of bit 4 280: =00000020 BIT5 EQU %00100000 ; bit 5 281: =000000DF INV5 EQU %11011111 ; inverse of bit 5 282: =00000040 BIT6 EQU %01000000 ; bit 6 283: =000000BF INV6 EQU %10111111 ; inverse of bit 6 284: =00000080 BIT7 EQU %10000000 ; bit 7 285: =0000007F INV7 EQU %01111111 ; inverse of bit 7 286: * 287: ************************************************************ 288: * Port Equates 289: ************************************************************ 290: * 291: =00004000 SEG7PORT EQU $4000 ; 7-segment display output port 292: *OUT1PORT EQU $5000 ; Output port 1 293: =00006000 LED1PORT EQU $6000 ; LED Output port 1 294: *LED2PORT EQU $7000 ; LED Output port 2 295: * IN1PORT EQU $4000 ; Input port 1 296: * IN2PORT EQU $5000 ; Input port 2 297: * 298: ************************************************************ 299: * STEVE Common System Equates 300: ************************************************************ 301: * 302: 303: * Object avoidance/following reaction equates 304: ********************************************* 305: =00000001 REACTBACK EQU $01 ; Reaction values 306: =00000002 REACTHARD EQU $02 ; 307: =00000003 REACTSOFT EQU $03 ; 308: =00000000 NOACTION EQU $00 ; 309: 310: =00000000 REACTRAND EQU $00 ; 311: =00000001 REACTLEFT EQU $01 ; 312: =00000002 REACTRIGHT EQU $02 ; 313: 314: #endif 15: 16: * 17: ************************************************************ 18: * Main Equates 19: ************************************************************ 20: * 21: =0000B000 PROGSTART EQU $B000 ; start of the program 22: =00000A00 STACKPTR EQU $0A00 ; bottom of internal RAM for stack 23: =00000900 GLBLVARS EQU $0900 ; top of internal RAM for global variables 24: 25: =0000000A OAPROCRATE EQU 10 ; how often to execute object avoidance (in ms) 26: =0000000A FPROCRATE EQU 10 ; how often to execute following (in ms) 27: * 28: ************************************************************ 29: * Global Variables 30: ************************************************************ 31: * 32: =00000900 ORG GLBLVARS 33: 0900 0000 UPPERTIMER DC.W $0000 ; 16-bit extension of TCNT 34: 0902 0000 LEFTSPDTIME DC.W $0000 ; Time left break beam was broken 35: 0904 0000 RIGHTSPDTIME DC.W $0000 ; Time right break beam was broken 36: 0906 00 SPDTIMEFLG DC.B $00 ; Flags to indicate which beams were broken 37: 0907 00 SPDCAUGHT DC.B $00 ; Flags to indicate speeder caught 38: 0908 00 FLASHTIMER DC.B $00 ; Timer for flashing lights 39: 0909 00 FLASHPREV DC.B $00 ; Previous status of lights 40: 090A 0000 SIRENFREQ DC.W $0000 ; Frequency of the siren 41: 090C 0000 SIRENTIMER DC.W $0000 ; Timer for siren 42: 43: 090E 0000 OLEFTSPD DC.W $0000 ; current speed of left motor 44: 0910 0000 NLEFTSPD DC.W $0000 ; next speed of left motor 45: 0912 0000 ORIGHTSPD DC.W $0000 ; current speed of right motor 46: 0914 0000 NRIGHTSPD DC.W $0000 ; next speed of right motor 47: 48: 0916 0000 MAXFOWARDSPD DC.W $0000 ; maximum foward speed for motors 49: 0918 0000 MAXBACKSPD DC.W $0000 ; maximum reverse speed for motors 50: 091A 00 LVALUE DC.B $00 ; value of left IR 51: 091B 00 LREACT DC.B $00 ; reaction to left IR value 52: 091C 00 RVALUE DC.B $00 ; value of right IR 53: 091D 00 RREACT DC.B $00 ; reaction to right IR value 54: 091E 00 CVALUE DC.B $00 ; value of center IR 55: 091F 00 CREACT DC.B $00 ; reaction to center IR value 56: 0920 00 PREVRAND DC.B $00 ; previous random turning direction 57: 0921 00 PREVREACT DC.B $00 ; previous reaction value 58: 0922 0000 CURRMAN DC.W $0000 ; current manuever being performed 59: 60: 0924 00 LFVALUE DC.B $00 ; left follow sensor value 61: 0925 00 CFVALUE DC.B $00 ; center follow sensor value 62: 0926 00 RFVALUE DC.B $00 ; right follow sensor value 63: 0927 00 FTBLIDX DC.B $00 ; index into reaction table 64: 0928 00 LASTFOLLOW DC.B $00 ; last follow direction 65: 0929 0000 OLDCFAVG DC.W $0000 ; old average of center follow sensor values 66: 092B 0000 NEWCFAVG DC.W $0000 ; new average of center follow sensor values 67: 092D 00 AVGCNT DC.B $0000 ; number of items in new average 68: 69: 092E 00 BUMPVALUE DC.B $00 ; A/D bumper value from wait function 70: 71: * 72: ************************************************************ 73: * Pseudointerrupt vectors 74: ************************************************************ 75: * 76: =00000830 ORG T0_PVECT 77: 0830 06 B38E [03] JMP LEFTSTS 78: 79: =0000082D ORG T1_PVECT 80: 082D 06 B3D2 [03] JMP RIGHTSTS 81: 82: =00000818 ORG TMR_OVER_PVECT 83: 0818 06 B416 [03] JMP TIMEEXT 84: 85: =00000833 ORG RTI_PVECT 86: 0833 06 B466 [03] JMP UPDMOTORS 87: 88: =0000081E ORG T6_PVECT 89: 081E 06 B476 [03] JMP SIREN 90: 91: =0000084B ORG USERPROG_PVECT 92: 084B 06 B0DD [03] JMP MAIN 93: 94: =0000B000 ORG PROGSTART 95: * 96: ************************************************************ 97: * Main Constants 98: ************************************************************ 99: * 100: B000 0D 0A WELCOME DC.B CR,LF 101: B002 53 54 45 56 45 20 DC.B 'STEVE - Speed Trap Enforcement VehiclE' B008 2D 20 53 70 65 65 B00E 64 20 54 72 61 70 B014 20 45 6E 66 6F 72 B01A 63 65 6D 65 6E 74 B020 20 56 65 68 69 63 B026 6C 45 102: B028 0D 0A DC.B CR,LF 103: B02A 4D 69 63 68 61 65 DC.B 'Michael Hattermann' B030 6C 20 48 61 74 74 B036 65 72 6D 61 6E 6E 104: B03C 0D 0A DC.B CR,LF 105: B03E 49 4D 44 4C 20 2D DC.B 'IMDL - Spring 2002' B044 20 53 70 72 69 6E B04A 67 20 32 30 30 32 106: B050 0D 0A 04 DC.B CR,LF,EOS 107: B053 0D 0A SHUTDOWN DC.B CR,LF 108: B055 53 54 45 56 45 20 DC.B 'STEVE - Program ended...shutting down systems' B05B 2D 20 50 72 6F 67 B061 72 61 6D 20 65 6E B067 64 65 64 2E 2E 2E B06D 73 68 75 74 74 69 B073 6E 67 20 64 6F 77 B079 6E 20 73 79 73 74 B07F 65 6D 73 109: B082 0D 0A 04 DC.B CR,LF,EOS 110: B085 4C 65 66 74 20 49 LEFTIRSTR DC.B 'Left IR value = ' B08B 52 20 76 61 6C 75 B091 65 20 3D 20 111: B095 04 DC.B EOS 112: B096 52 69 67 68 74 20 RIGHTIRSTR DC.B 'Right IR value = ' B09C 49 52 20 76 61 6C B0A2 75 65 20 3D 20 113: B0A7 04 DC.B EOS 114: B0A8 0D 0A 04 NEWLINE DC.B CR,LF,EOS 115: B0AB 4C 65 66 74 20 62 LEFTSPSTR DC.B 'Left beam broken' B0B1 65 61 6D 20 62 72 B0B7 6F 6B 65 6E 116: B0BB 0D 0A 04 DC.B CR,LF,EOS 117: B0BE 52 69 67 68 74 20 RIGHTSPSTR DC.B 'Right beam broken' B0C4 62 65 61 6D 20 62 B0CA 72 6F 6B 65 6E 118: B0CF 0D 0A 04 DC.B CR,LF,EOS 119: B0D2 0D 0A SPEED DC.B CR,LF 120: B0D4 53 50 45 45 44 20 DC.B 'SPEED = ' B0DA 3D 20 121: B0DC 04 DC.B EOS 122: 123: 124: * 125: ******************************************************************************* 126: * SUBROUTINE - MAIN 127: * Description: Main program. Inits the robots sub-systems and begins the robot 128: * behavior code. 129: * Input : None. 130: * Output : None. 131: * Destroys : None. 132: * Calls : None. 133: ******************************************************************************* 134: * 135: B0DD 180B 00 0016 [04] MAIN MOVB #$00,COPCTL ; turn off COP watchdog timer 136: B0E2 CF 0A00 [02] LDS #STACKPTR ; load stack pointer 137: 138: B0E5 16 B23B [04] JSR INITSCI ; init SCI system 139: B0E8 16 B249 [04] JSR INITATD ; init A/D system 140: B0EB 16 B275 [04] JSR INITTIME ; init timer system 141: B0EE 180B 00 6000 [04] MOVB #$00,LED1PORT ; turn off flashing lights 142: B0F3 180B 00 4000 [04] MOVB #$00,SEG7PORT ; reset speed capture to zero 143: 144: B0F8 CE B000 [02] LDX #WELCOME ; print welcome message 145: B0FB 16 B1EF [04] JSR OUTSTR ; 146: 147: B0FE 10EF [01] CLI ; turn on interrupts 148: 149: B100 16 B351 [04] JSR WAITSPEED ; wait for a speeder 150: B103 16 B487 [04] JSR INITPWM ; init PWM system 151: B106 16 B35E [04] JSR FLASHON ; turn on flashing lights 152: B109 16 B36A [04] JSR SIRENON ; turn on siren 153: B10C 16 B5CE [04] JSR PULLOUT ; pull out and prepare to follow 154: 155: B10F MAIN2 156: B10F 16 B7B5 [04] JSR FOLLOW ; follow speeder 157: B112 CE 000A [02] LDX #FPROCRATE ; wait designated amount of time 158: B115 16 B171 [04] JSR WAIT ; 159: B118 20 F5 [03] BRA MAIN2 ; do it again 160: 161: * 162: ******************************************************************************* 163: * SUBROUTINE - MAINOUT 164: * Description: Exits the program. Stops motors, signals end of program with 165: * the lights, then shuts down the subsystems and enters a 166: * never ending loop 167: * Input : None. 168: * Output : None. 169: * Destroys : None. 170: * Calls : STEER,OUTSTR,WAIT,KILLATD,KILLPWM,KILLTIME. 171: ******************************************************************************* 172: * 173: B11A CE 0008 [02] MAINOUT LDX #STOP ; stop the motors 174: B11D 16 B53D [04] JSR STEER ; 175: B120 CE B053 [02] LDX #SHUTDOWN ; print shutdown message 176: B123 16 B1EF [04] JSR OUTSTR ; 177: 178: B126 86 08 [01] LDAA #8 ; load loop counter 179: B128 180B 3C 6000 [04] MAINOUT1 MOVB #$3C,LED1PORT ; turn off lights 180: B12D 16 B37F [04] JSR SIRENOFF ; turn off siren 181: B130 CE 00FA [02] LDX #250 ; wait 182: B133 16 B171 [04] JSR WAIT ; 183: B136 180B C3 6000 [04] MOVB #$C3,LED1PORT ; turn on lights 184: B13B 16 B36A [04] JSR SIRENON ; turn on siren 185: B13E CE 00FA [02] LDX #250 ; wait 186: B141 16 B171 [04] JSR WAIT ; 187: B144 04 30 E1 [03] DBNE A,MAINOUT1 ; continue looping until done 188: B147 180B 00 6000 [04] MOVB #$00,LED1PORT ; turn on lights 189: 190: B14C 16 B261 [04] JSR KILLATD ; shutdown atd system 191: B14F 16 B4D8 [04] JSR KILLPWM ; shutdown pwm system 192: B152 16 B328 [04] JSR KILLTIME ; shutdown timer system 193: B155 180B 00 6000 [04] MOVB #$00,LED1PORT ; turn off flashing lights 194: B15A 180B 00 4000 [04] MOVB #$00,SEG7PORT ; clear 7 segment display 195: B15F 20 FE [03] MAINOUTX BRA MAINOUTX ; end of program 196: * 197: ******************************************************************************* 198: 199: #include "wait.asm" 1: * Filename : WAIT.ASM 2: * Programmer : Michael Hattermann 3: * Date : March 29, 2002 4: * Version : 1.0 5: * Description : This file contains the wait and bumper 6: * functions. They must be together because 7: * the bumpers are checked while waiting. The 8: * following functions are available: 9: * 10: * WAIT - waits for specified # ms 11: * BUMPED - Determines if a bump has occured 12: * 13: *#define __DEBUGBUMP_ 1 14: =00000001 #define __PRINTBUMP_ 1 15: 16: #include "hc12.asm" 1: * Filename : HC12.H 2: * Programmer : Michael Hattermann 3: * Date : February 21, 2002 4: * Version : 1.0 5: * Description : This file contains equates for the HC12 6: * processor 7: * 8: * 9: ************************************************************ 10: * Register Equates 11: ************************************************************ 12: * 13: #ifndef __HC12_ASM_ 314: #endif 17: * 18: ************************************************************ 19: * Bump/Wait Equates 20: ************************************************************ 21: * 22: =00000009 NOBUMPMAX EQU $09 ; max value for no bumper pressed 23: =00000047 FRONTREAR EQU $47 ; division between front and back 24: 25: * 26: ************************************************************ 27: * Bump/Wait Debug Code 28: ************************************************************ 29: * 30: #ifdef __DEBUGBUMP_ 77: #endif 78: 79: #ifdef __PRINTBUMP_ 80: B161 42 55 4D 50 45 52 BUMPSTR DC.B 'BUMPER VALUE = ' B167 20 56 41 4C 55 45 B16D 20 3D 20 81: B170 04 DC.B EOS 82: #endif 83: 84: * 85: ******************************************************************************* 86: * SUBROUTINE - WAIT 87: * Description: Waits for the designated amount of time (in ms). If a bumper is 88: * pressed while waiting, it will quit waiting and returns to the 89: * function that called it. Function returns 0 if waited full time, 90: * returns bumper value otherwise 91: * Input : # of ms to wait in reg X. 92: * Output : Bumper value in BUMPVALUE. 93: * Destroys : None. 94: * Calls : None. 95: ******************************************************************************* 96: * 97: B171 04 45 21 [03] WAIT TBEQ X,WAITX ; if no time to wait, get out 98: B174 34 [02] PSHX ; save reg X 99: B175 3B [02] PSHD ; save reg D 100: B176 180B 00 092E [04] MOVB #$00,BUMPVALUE ; clear old bumper value 101: 102: B17B C6 0B [01] WAIT1 LDAB #11 ; load loop counter 103: 104: B17D 86 07 [01] WAIT2 LDAA #BUMPER ; check to see if we have 105: B17F 16 B26C [04] JSR ANALOG ; been bumped 106: B182 81 09 [01] CMPA #NOBUMPMAX ; were we bumped? 107: B184 22 12 [03] BHI WAITBX ; yes, get out 108: B186 A7 [01] NOP ; do nothing 109: B187 A7 [01] NOP ; 110: B188 A7 [01] NOP ; 111: B189 A7 [01] NOP ; 112: B18A A7 [01] NOP ; 113: B18B A7 [01] NOP ; 114: B18C A7 [01] NOP ; 115: B18D A7 [01] NOP ; 116: B18E A7 [01] NOP ; 117: B18F 04 31 EB [03] DBNE B,WAIT2 ; repeat until counter=0 118: 119: B192 04 35 E6 [03] DBNE X,WAIT1 ; if we need to wait more, go wait 120: 121: B195 3A [03] WAITX PULD ; restore reg D 122: B196 30 [03] PULX ; restore reg X 123: B197 3D [05] RTS ; return to caller 124: B198 7A 092E [03] WAITBX STAA BUMPVALUE ; save bumper value 125: 126: #ifdef __PRINTBUMP_ 127: B19B 34 [02] PSHX ; save reg X 128: B19C CE B161 [02] LDX #BUMPSTR ; print bump string 129: B19F 16 B1EF [04] JSR OUTSTR ; 130: B1A2 16 B20D [04] JSR OUTNUM ; print analog value 131: B1A5 CE B0A8 [02] LDX #NEWLINE ; print end of line 132: B1A8 16 B1EF [04] JSR OUTSTR ; 133: B1AB 30 [03] PULX ; restore reg X 134: #endif 135: 136: B1AC 20 E7 [03] BRA WAITX ; get out 137: 138: ******************************************************************************* 139: * SUBROUTINE - BUMPED 140: * Description: Determines if a bump sensor has been pressed. If it has, the 141: * function will return the value read from the bumper A/D port. 142: * If no bumper is pressed, a $00 will be returned. 143: * Input : None. 144: * Output : Bumper value in reg A. 145: * Destroys : Reg A. 146: * Calls : None. 147: ******************************************************************************* 148: * 149: B1AE B6 092E [03] BUMPED LDAA BUMPVALUE ; get the last bump value 150: B1B1 3D [05] RTS ; return to caller 151: * 152: ******************************************************************************* 153: * SUBROUTINE - WAIT 154: * Description: Waits for the designated amount of time (in ms). 155: * Input : # of ms to wait in reg X. 156: * Output : None. 157: * Destroys : None. 158: * Calls : None. 159: ******************************************************************************* 160: * 161: *WAIT TBEQ X,WAITX ; if no time to wait, get out 162: * PSHX ; save reg X 163: * PSHD ; save reg D 164: * 165: *WAIT1 LDD #1323 ; load loop counter 166: * 167: *WAIT2 168: * DBNE D,WAIT2 ; repeat until counter=0 169: * 170: * DBNE X,WAIT1 ; if we need to wait more, go wait 171: * 172: *WAITX PULD ; restore reg D 173: * PULX ; restore reg X 174: * RTS ; return to caller 175: * 176: ******************************************************************************* 200: #include "sci.asm" 1: * Filename : SCI.ASM 2: * Programmer : Michael Hattermann 3: * Date : February 4, 2002 4: * Version : 1.0 5: * Description : This file contains SCI communication 6: * functions for input and output of 7: * data. The following functions are 8: * available: 9: * 10: * WAIT_TC - wait for transmit complete 11: * SET_BAUD - change the baud rate 12: * TX_ON - turn transmitter on 13: * TX_OFF - turn transmitter off 14: * RX_ON - turn receiver on 15: * RX_OFF - turn receiver off 16: * RX_INT_ON - turn receiver interrupts on 17: * RX_INT_OFF - turn receiver interrupts off 18: * OUTCHAR - prints character to screen 19: * OUTSTR - prints string to screen 20: * INCHARWAIT - waits for character input 21: * INCHAR - get character input if any 22: * OUTNUM - prints number to screen 23: * NIBTOCHAR - prints nibble to screen 24: * OUTADDR - prints 16-bit num to screen 25: * INITSCI - turns on SCI for 9600 baud 26: * 27: * 28: *#define __DEBUGSCI_ 1 29: 30: #include "hc12.asm" 1: * Filename : HC12.H 2: * Programmer : Michael Hattermann 3: * Date : February 21, 2002 4: * Version : 1.0 5: * Description : This file contains equates for the HC12 6: * processor 7: * 8: * 9: ************************************************************ 10: * Register Equates 11: ************************************************************ 12: * 13: #ifndef __HC12_ASM_ 314: #endif 31: 32: * 33: ************************************************************ 34: * SCI Equates 35: ************************************************************ 36: * 37: ****Baud rate equates**** 38: =00000000 BAUD19200 EQU 0 39: =00000002 BAUD14400 EQU 2 40: =00000004 BAUD9600 EQU 4 41: =00000006 BAUD4800 EQU 6 42: =00000008 BAUD2400 EQU 8 43: =0000000A BAUD1200 EQU 10 44: =0000000C BAUD600 EQU 12 45: =0000000E BAUD300 EQU 14 46: 47: ****ASCII character equates**** 48: =00000004 EOS EQU $04 ; User-defined End Of String (EOS) character 49: =0000000D CR EQU $0D ; Carriage Return Character 50: =0000000A LF EQU $0A ; Line Feed Character 51: =0000001B ESC EQU $1B ; ESC character 52: 53: * 54: ************************************************************ 55: * SCI Test Program 56: ************************************************************ 57: * 58: #ifdef __DEBUGSCI_ 105: #endif 106: * 107: ************************************************************ 108: * Constant Definitions 109: ************************************************************ 110: * 111: B1B2 000D BAUDTBL DC.W 13 ; (0) BAUD rate = 19200 112: B1B4 0011 DC.W 17 ; (1) BAUD rate = 14400 113: B1B6 001A DC.W 26 ; (2) BAUD rate = 9600 114: B1B8 0034 DC.W 52 ; (3) BAUD rate = 4800 115: B1BA 0068 DC.W 104 ; (4) BAUD rate = 2400 116: B1BC 00D0 DC.W 208 ; (5) BAUD rate = 1200 117: B1BE 01A1 DC.W 417 ; (6) BAUD rate = 600 118: B1C0 0341 DC.W 833 ; (7) BAUD rate = 300 119: * 120: ******************************************************************************* 121: * SUBROUTINE - WAIT_TC 122: * Description: Waits for the current transmit operation to complete (polls the 123: * TC flag in SCI status register 1) 124: * Input : None. 125: * Output : None. 126: * Destroys : None. 127: * Calls : None. 128: ******************************************************************************* 129: * 130: B1C2 WAIT_TC 131: B1C2 4F C4 40 FC [04] BRCLR SC0SR1,BIT6,WAIT_TC ; wait until done sending 132: B1C6 3D [05] RTS ; Return to caller 133: * 134: * 135: ******************************************************************************* 136: * SUBROUTINE - SET_BAUD 137: * Description: Sets the baud rate to the rate specified in register A. Reg A 138: * can only take on these predefined values: 139: * BAUD19200 = BAUD rate 19200 140: * BAUD14400 = BAUD rate 14400 141: * BAUD9600 = BAUD rate 9600 142: * BAUD4800 = BAUD rate 4800 143: * BAUD2400 = BAUD rate 2400 144: * BAUD1200 = BAUD rate 1200 145: * BAUD600 = BAUD rate 600 146: * BAUD300 = BAUD rate 300 147: * 148: * Input : New baud rate in reg A 149: * Output : None. 150: * Destroys : SC0BDH, SC0BDL. 151: * Calls : None. 152: ******************************************************************************* 153: * 154: B1C7 SET_BAUD 155: B1C7 34 [02] PSHX ; Preserve reg X 156: 157: B1C8 CE B1B2 [02] LDX #BAUDTBL ; Load address of baud table 158: B1CB EE E4 [03] LDX A,X ; Load baud rate from table 159: B1CD 5E C0 [02] STX SC0BD ; Set baud rate in register 160: 161: B1CF 30 [03] PULX ; Restore reg X 162: B1D0 3D [05] RTS ; Return to caller 163: * 164: ******************************************************************************* 165: * SUBROUTINE - TX_ON, TX_OFF 166: * Description: Enables transmitter, disables transmitter 167: * Input : None. 168: * Output : None. 169: * Destroys : SC0CR2. 170: * Calls : None. 171: ******************************************************************************* 172: * 173: B1D1 TX_ON 174: B1D1 4C C3 08 [04] BSET SC0CR2,BIT3 ; turn on the transmitter 175: B1D4 3D [05] RTS ; return to caller 176: B1D5 TX_OFF 177: B1D5 4D C3 08 [04] BCLR SC0CR2,BIT3 ; turn off the transmitter 178: B1D8 3D [05] RTS ; return to caller 179: * 180: ******************************************************************************* 181: * SUBROUTINE - RX_ON, RX_OFF,RX_INT_ON,RX_INT_OFF 182: * Description: Enables receiver, disables receiver, enables receive interrupts, 183: * disables receive interrupts 184: * Input : None. 185: * Output : None. 186: * Destroys : SC0CR2. 187: * Calls : None. 188: ******************************************************************************* 189: * 190: B1D9 RX_ON 191: B1D9 4C C3 04 [04] BSET SC0CR2,BIT2 ; turn on the receiver 192: B1DC 3D [05] RTS ; return to caller 193: B1DD RX_OFF 194: B1DD 4D C3 04 [04] BCLR SC0CR2,BIT2 ; turn off the receiver 195: B1E0 3D [05] RTS ; return to caller 196: B1E1 RX_INT_ON 197: B1E1 4C C3 20 [04] BSET SC0CR2,BIT5 ; enable receiver interrupts 198: B1E4 3D [05] RTS ; return to caller 199: B1E5 RX_INT_OFF 200: B1E5 4D C3 20 [04] BCLR SC0CR2,BIT5 ; disable receiver interrupts 201: B1E8 3D [05] RTS ; return to caller 202: * 203: ************************************************************************ 204: * SUBROUTINE - OUTCHAR 205: * Description: Outputs the character in register A to the screen 206: * Input : Data to be transmitted in register A. 207: * Output : Transmits the data. 208: * Destroys : None. 209: * Calls : WAIT_TC 210: ************************************************************************ 211: * 212: B1E9 OUTCHAR 213: B1E9 16 B1C2 [04] JSR WAIT_TC ; wait until transmitter is idle 214: B1EC 5A C7 [02] STAA SC0DRL ; output character 215: B1EE 3D [05] RTS ; Return from subtoutine 216: * 217: ************************************************************************ 218: * SUBROUTINE - OUTSTR 219: * Description: Outputs the string pointed to by X. String must be 220: * terminated by EOS character. 221: * Input : String to be output in reg X 222: * Output : Transmits the string. 223: * Destroys : None. 224: * Calls : OUTCHAR 225: ************************************************************************ 226: * 227: B1EF OUTSTR 228: B1EF 36 [02] PSHA ; preserve reg A 229: B1F0 34 [02] PSHX ; preserve reg X 230: B1F1 OUTSTR1 231: B1F1 A6 30 [03] LDAA 1,X+ ; Get a character (put in reg A) 232: B1F3 81 04 [01] CMPA #EOS ; Check if it's EOS 233: B1F5 27 05 [03] BEQ OUTSTR2 ; Branch to Done if it's EOS 234: B1F7 16 B1E9 [04] JSR OUTCHAR ; Print the character 235: B1FA 20 F5 [03] BRA OUTSTR1 236: B1FC OUTSTR2 237: B1FC 30 [03] PULX ; restore reg X 238: B1FD 32 [03] PULA ; restore reg A 239: B1FE 3D [05] RTS ; Return from subtoutine 240: * 241: ************************************************************************ 242: * SUBROUTINE - INCHARWAIT 243: * Description: Waits for a character to be pressed and reads it into 244: * reg A 245: * Input : None 246: * Output : Character pressed in reg. A 247: * Destroys : A. 248: * Calls : None 249: ************************************************************************ 250: * 251: B1FF INCHARWAIT 252: B1FF 4F C4 20 FC [04] BRCLR SC0SR1,BIT5,INCHARWAIT ; wait until buffer full 253: B203 96 C7 [03] LDAA SC0DRL ; input character 254: B205 3D [05] RTS ; Return from subroutine 255: * 256: ************************************************************************ 257: * SUBROUTINE - INCHAR 258: * Description: Checks to see if character recevied - if so returns the 259: * character, if not returns 0 260: * Input : None 261: * Output : Character pressed in reg A; 0 if none 262: * Destroys : A. 263: * Calls : None 264: ************************************************************************ 265: * 266: B206 INCHAR 267: B206 4F C4 20 02 [04] BRCLR SC0SR1,BIT5,INCHAR1 ; if there is no data, get out 268: B20A 96 C7 [03] LDAA SC0DRL ; yes, read data 269: B20C INCHAR1 270: B20C 3D [05] RTS ; return to caller 271: * 272: ************************************************************************ 273: * SUBROUTINE - OUTNUM 274: * Description: Outputs the number in register A to the screen 275: * Input : Data to be transmitted in register A. 276: * Output : Transmits the data. 277: * Destroys : None. 278: * Calls : NIBTOCHAR 279: ************************************************************************ 280: * 281: B20D OUTNUM 282: B20D 36 [02] PSHA ; preserve reg A 283: B20E 36 [02] PSHA ; preserve reg A 284: B20F 84 F0 [01] ANDA #%11110000 ; get upper nibble 285: B211 44 [01] LSRA ; shift it right to get the nibble 286: B212 44 [01] LSRA 287: B213 44 [01] LSRA 288: B214 44 [01] LSRA 289: B215 16 B220 [04] JSR NIBTOCHAR ; change A and print it 290: B218 32 [03] PULA ; restore reg A 291: B219 84 0F [01] ANDA #%00001111 ; get lower nibble 292: B21B 16 B220 [04] JSR NIBTOCHAR ; change A and print it 293: B21E 32 [03] PULA ; restore reg A 294: B21F 3D [05] RTS ; return to caller 295: * 296: ************************************************************************ 297: * SUBROUTINE - NIBTOCHAR 298: * Description: Converts lower nibble of A to ASCII and prints it 299: * Input : Data to convert in A. 300: * Output : Transmits the data. 301: * Destroys : None. 302: * Calls : OUTCHAR 303: ************************************************************************ 304: * 305: B220 NIBTOCHAR 306: B220 81 09 [01] CMPA #9 ; is it greater than 9? 307: B222 2E 04 [03] BGT NIBTOCHAR1 ; if so, print a character 308: B224 8B 30 [01] ADDA #48 ; if not, print a number starting at 48 ASCII 309: B226 20 02 [03] BRA NIBTOCHAR2 ; 310: B228 NIBTOCHAR1 311: B228 8B 37 [01] ADDA #55 ; if so, print a letter starting at 55 = 65-10 312: B22A NIBTOCHAR2 313: B22A 16 B1E9 [04] JSR OUTCHAR ; print it 314: B22D 3D [05] RTS 315: * 316: ************************************************************************ 317: * SUBROUTINE - OUTADDR 318: * Description: Outputs the number in reg X to the screen 319: * Input : Data to print in X. 320: * Output : Transmits the data. 321: * Destroys : None. 322: * Calls : OUTNUM 323: ************************************************************************ 324: * 325: B22E OUTADDR 326: B22E 3B [02] PSHD ; save reg D 327: B22F B7 54 [01] TFR X,D ; load X into D 328: B231 16 B20D [04] JSR OUTNUM ; prints whats in A -- MSB 329: B234 180F [02] TBA ; B -> A 330: B236 16 B20D [04] JSR OUTNUM ; prints whats in B -- LSB 331: B239 3A [03] PULD ; restore D 332: B23A 3D [05] RTS ; return to caller 333: * 334: ************************************************************************ 335: * SUBROUTINE - INITSCI 336: * Description: This subroutine initializes the BAUD rate to 9600 and 337: * sets up the SCI port for 1 start bit, 8 data bits and 338: * 1 stop bit. It also enables the transmitter and receiver 339: * Input : None. 340: * Output : Initializes SCI. 341: * Destroys : None. 342: * Calls : SET_BAUD,TX_ON,RX_ON 343: ************************************************************************ 344: * 345: B23B 36 [02] INITSCI PSHA ; save reg A 346: B23C 86 04 [01] LDAA #BAUD9600 ; set the baud rate to 9600 347: B23E 16 B1C7 [04] JSR SET_BAUD ; 348: B241 16 B1D1 [04] JSR TX_ON ; turn on the transmitter 349: B244 16 B1D9 [04] JSR RX_ON ; turn on the receiver 350: B247 32 [03] PULA ; restore reg A 351: B248 3D [05] RTS ; Return from subtoutine 352: * 353: ************************************************************************ 201: #include "atd.asm" 1: * Filename : ATD.ASM 2: * Programmer : Michael Hattermann 3: * Date : February 22, 2002 4: * Version : 1.0 5: * Description : This file contains the Analog to 6: * Digital (A/D) conversion functions 7: * for the input of analog signals. The 8: * following functions are available: 9: * 10: * INITATD - Initializes the ATD system 11: * KILLATD - Shuts down the ATD system 12: * ANALOG - Returns ATD value for port specified 13: * 14: *#define __DEBUGATD_ 1 15: 16: 17: #include "hc12.asm" 1: * Filename : HC12.H 2: * Programmer : Michael Hattermann 3: * Date : February 21, 2002 4: * Version : 1.0 5: * Description : This file contains equates for the HC12 6: * processor 7: * 8: * 9: ************************************************************ 10: * Register Equates 11: ************************************************************ 12: * 13: #ifndef __HC12_ASM_ 314: #endif 18: * 19: ************************************************************ 20: * A/D Equates 21: ************************************************************ 22: * 23: =00000000 CHANNEL0 EQU $00 24: =00000001 CHANNEL1 EQU $01 25: =00000002 CHANNEL2 EQU $02 26: =00000003 CHANNEL3 EQU $03 27: =00000004 CHANNEL4 EQU $04 28: =00000005 CHANNEL5 EQU $05 29: =00000006 CHANNEL6 EQU $06 30: =00000007 CHANNEL7 EQU $07 31: * 32: ************************************************************ 33: * A/D Channel Assignments 34: ************************************************************ 35: * 36: =00000000 LEFTIR EQU CHANNEL0 37: =00000001 RIGHTIR EQU CHANNEL1 38: =00000002 CENTERIR EQU CHANNEL2 39: =00000004 CENTERFOLLOW EQU CHANNEL4 40: =00000005 LEFTFOLLOW EQU CHANNEL5 41: =00000006 RIGHTFOLLOW EQU CHANNEL6 42: =00000007 BUMPER EQU CHANNEL7 43: * 44: ************************************************************ 45: * A/D Debug Code 46: ************************************************************ 47: * 48: #ifdef __DEBUGATD_ 95: #endif 96: * 97: ******************************************************************************* 98: * SUBROUTINE - INITATD 99: * Description: Initializes the analog to digital converter 100: * Input : None. 101: * Output : None. 102: * Destroys : None. 103: * Calls : None. 104: ******************************************************************************* 105: * 106: B249 36 [02] INITATD PSHA ; save reg A 107: B24A 180B 80 0062 [04] MOVB #$80,ATDCTL2 ; turn on ATD system 108: B24F 180B 00 0063 [04] MOVB #$00,ATDCTL3 ; enable conversions in bgnd mode 109: B254 180B 01 0064 [04] MOVB #$01,ATDCTL4 ; setup conversion rate = 2MHz 110: 111: B259 86 C3 [01] LDAA #195 ; load loop counter 112: B25B A7 [01] INITATD1 NOP ; wait for ATD to power up 113: B25C 04 30 FC [03] DBNE A,INITATD1 ; if we still need to wait, wait 114: 115: B25F 32 [03] PULA ; restore reg A 116: B260 3D [05] RTS ; return to caller 117: * 118: ******************************************************************************* 119: * SUBROUTINE - KILLATD 120: * Description: Shuts down the analog to digital converter 121: * Input : None. 122: * Output : None. 123: * Destroys : None. 124: * Calls : None. 125: ******************************************************************************* 126: * 127: B261 180B 00 0060 [04] KILLATD MOVB #$00,ATDCTL0 ; stop current conversion (if there is one) 128: B266 180B 00 0062 [04] MOVB #$00,ATDCTL2 ; turn off ATD system 129: B26B 3D [05] RTS ; return to caller 130: * 131: ******************************************************************************* 132: * SUBROUTINE - ANALOG 133: * Description: Converts the analog channel specified by reg A and returns the 134: * converted value in reg A. Valid values for channel are (the 135: * equates an be found above): 136: * 137: * CHANNEL0 - A/D Channel #0 138: * CHANNEL1 - A/D Channel #1 139: * CHANNEL2 - A/D Channel #2 140: * CHANNEL3 - A/D Channel #3 141: * CHANNEL4 - A/D Channel #4 142: * CHANNEL5 - A/D Channel #5 143: * CHANNEL6 - A/D Channel #6 144: * CHANNEL7 - A/D Channel #7 145: * 146: * Input : Channel to convert in reg A. 147: * Output : Digital value of channel in reg A. 148: * Destroys : None. 149: * Calls : None. 150: ******************************************************************************* 151: * 152: B26C 5A 65 [02] ANALOG STAA ATDCTL5 ; start conversion on channel specified 153: B26E 4F 66 80 FC [04] ANALOG1 BRCLR ATDSTATH,BIT7,ANALOG1 ; wait for conversion to complete 154: B272 96 74 [03] LDAA ADR2H ; load conversion result 155: B274 3D [05] RTS ; return to caller 156: * 157: ************************************************************************ 202: #include "time.asm" 1: * Filename : TIME.ASM 2: * Programmer : Michael Hattermann 3: * Date : February 21, 2002 4: * Version : 1.0 5: * Description : This file contains time functions. The 6: * following functions are available: 7: * 8: * INITTIME - initialize timer system 9: * KILLTIME - shut down timer system 10: * WAITSPEED - waits for a speeder to go by 11: * FLASHON - turns on flashing lights 12: * FLASHOFF - turns off flashing lights 13: * SIRENON - turns on the siren 14: * SIRENOFF - turns off the siren 15: * LEFTSTS - handles left speed trap sensor 16: * RIGHTSTS - handles right speed trap sensor 17: * TIMEEXT - handles extended timer,flashing lights,siren 18: * UPDMOTORS - updates speed on motors 19: * SIREN - handles siren output 20: * 21: *#define __DEBUGTIME_ 1 22: *#define __PRINTTIME_ 1 23: 24: #include "hc12.asm" 1: * Filename : HC12.H 2: * Programmer : Michael Hattermann 3: * Date : February 21, 2002 4: * Version : 1.0 5: * Description : This file contains equates for the HC12 6: * processor 7: * 8: * 9: ************************************************************ 10: * Register Equates 11: ************************************************************ 12: * 13: #ifndef __HC12_ASM_ 314: #endif 25: 26: * 27: ************************************************************ 28: * Time Equates 29: ************************************************************ 30: * 31: =00000001 LSPDFLAG EQU BIT0 ; Flags to indicate if a 32: =00000002 RSPDFLAG EQU BIT1 ; beam was broken 33: =00000003 BSPDFLAG EQU 3 ; 34: 35: =0000001F SPDLIMIT EQU $1F ; speed limit 36: =0000000F FLASHRATE EQU 15 ; flashing light rate (1/4 second) 37: =0000000F FLASHPAT1 EQU $0F ; 1st light pattern 38: =000000F0 FLASHPAT2 EQU $F0 ; 2nd light pattern 39: 40: =FFFFFFF6 SIRENRATE1 EQU -10 ; siren frequency change rate 41: =0000000A SIRENRATE2 EQU 10 ; siren frequency change rate 42: =000007D0 SIRENFREQ1 EQU 2000 ; first siren frequency 43: =000004B0 SIRENFREQ2 EQU 1200 ; second siren frequency 44: 45: * 46: ************************************************************ 47: * Time Debug Program 48: ************************************************************ 49: * 50: #ifdef __DEBUGTIME_ 113: #endif 114: * 115: ******************************************************************************* 116: * SUBROUTINE - INITTIME 117: * Description: Initializes the timer system 118: * TC0 - left speed trap IR receiver 119: * TC1 - right speed trap IR receiver 120: * TC6 - siren output 121: * Input : None. 122: * Output : None. 123: * Destroys : None. 124: * Calls : None. 125: ******************************************************************************* 126: * 127: B275 180B 40 0080 [04] INITTIME MOVB #$40,TIOS ; setup input capture/output compare lines 128: B27A 180B 00 0081 [04] MOVB #$00,CFORC ; setup timer compare force register 129: B27F 180B 00 0082 [04] MOVB #$00,OC7M ; setup OC7 mask register 130: B284 180B 00 0083 [04] MOVB #$00,OC7D ; setup OC7 data register 131: B289 180B 00 0088 [04] MOVB #$00,TCTL1 ; setup output compare pin action reg 1 132: B28E 180B 00 0089 [04] MOVB #$00,TCTL2 ; setup output compare pin action reg 2 133: B293 180B 00 008A [04] MOVB #$00,TCTL3 ; setup input capture edge detection reg 1 134: B298 180B 05 008B [04] MOVB #$05,TCTL4 ; setup input capture edge detection reg 2 135: B29D 180B 03 008C [04] MOVB #$03,TMSK1 ; setup interrupts on timer lines 136: B2A2 180B A0 008D [04] MOVB #$A0,TMSK2 ; setup misc timer pin options(pullups on IC pins) 137: B2A7 1803 0000 0090 [05] MOVW #$0000,TC0 ; clear TC0 register 138: B2AD 1803 0000 0092 [05] MOVW #$0000,TC1 ; clear TC1 register 139: B2B3 1803 0000 0094 [05] MOVW #$0000,TC2 ; clear TC2 register 140: B2B9 1803 0000 0096 [05] MOVW #$0000,TC3 ; clear TC3 register 141: B2BF 1803 0000 0098 [05] MOVW #$0000,TC4 ; clear TC4 register 142: B2C5 1803 0000 009A [05] MOVW #$0000,TC5 ; clear TC5 register 143: B2CB 1803 0000 009C [05] MOVW #$0000,TC6 ; clear TC6 register 144: B2D1 1803 0000 009E [05] MOVW #$0000,TC7 ; clear TC7 register 145: B2D7 180B FF 008E [04] MOVB #$FF,TFLG1 ; clear all interrupt flags 146: B2DC 180B 80 008F [04] MOVB #$80,TFLG2 ; clear interrupt flag 147: B2E1 1803 0000 0900 [05] MOVW #$0000,UPPERTIMER ; clear timer extension timer 148: B2E7 1803 0000 0902 [05] MOVW #$0000,LEFTSPDTIME ; clear left speed time 149: B2ED 1803 0000 0904 [05] MOVW #$0000,RIGHTSPDTIME ; clear left speed time 150: B2F3 180B 00 0908 [04] MOVB #$00,FLASHTIMER ; clear flashing lights timer 151: B2F8 180B 00 0906 [04] MOVB #$00,SPDTIMEFLG ; clear broken beam flags 152: B2FD 180B 00 0907 [04] MOVB #$00,SPDCAUGHT ; clear speeder caught flag 153: B302 180B 00 0909 [04] MOVB #$00,FLASHPREV ; turn off flashing lights 154: B307 1803 0000 090A [05] MOVW #$0000,SIRENFREQ ; clear siren frequency 155: B30D 1803 0000 090C [05] MOVW #$0000,SIRENTIMER ; clear siren timer 156: B313 180B 00 092E [04] MOVB #$00,BUMPVALUE ; clear bumper value 157: B318 180B 86 0014 [04] MOVB #$86,RTICTL ; set up, enable RTI 158: B31D 180B FF 0015 [04] MOVB #$FF,RTIFLG ; clear all RTI flags 159: B322 180B 80 0086 [04] MOVB #$80,TSCR ; enable the timer 160: B327 3D [05] RTS ; return to caller 161: * 162: ******************************************************************************* 163: * SUBROUTINE - KILLTIME 164: * Description: Shuts down the timer system 165: * Input : None. 166: * Output : None. 167: * Destroys : None. 168: * Calls : None. 169: ******************************************************************************* 170: * 171: B328 180B 00 0088 [04] KILLTIME MOVB #$00,TCTL1 ; disconnect all output compare pins 172: B32D 180B 00 0089 [04] MOVB #$00,TCTL2 ; 173: B332 180B 00 008A [04] MOVB #$00,TCTL3 ; disable all input capture pins 174: B337 180B 00 008B [04] MOVB #$00,TCTL4 ; 175: B33C 180B 00 008C [04] MOVB #$00,TMSK1 ; turn off interrupts 176: B341 180B 00 008D [04] MOVB #$00,TMSK2 ; turn off timer overflow interrupts 177: B346 180B 00 00A0 [04] MOVB #$00,PACTL ; turn off pulse accumulator 178: B34B 180B 00 0086 [04] MOVB #$00,TSCR ; turn off timer 179: B350 3D [05] RTS ; return to caller 180: * 181: ******************************************************************************* 182: * SUBROUTINE - WAITSPEED 183: * Description: Waits for a speeder to be caught and returns the direction the 184: * object was travelling (Reg A). Valid values are: 185: * LSPDFLAG - object was going left 186: * RSPDFLAG - object was going right 187: * 188: * Input : None. 189: * Output : Direction in reg A. 190: * Destroys : None. 191: * Calls : None. 192: ******************************************************************************* 193: * 194: B351 WAITSPEED 195: B351 B6 0907 [03] WAITSPEED1 LDAA SPDCAUGHT ; get flags for speeder caught 196: B354 04 50 FA [03] TBEQ A,WAITSPEED1 ; wait for a caught speeder 197: B357 4D 8C 01 [04] BCLR TMSK1,#BIT0 ; turn off left interrupts 198: B35A 4D 8C 01 [04] BCLR TMSK1,#BIT0 ; turn off right interrupts 199: B35D 3D [05] RTS ; return to caller 200: * 201: ******************************************************************************* 202: * SUBROUTINE - FLASHON 203: * Description: Turns the flashing lights on 204: * Input : None. 205: * Output : None. 206: * Destroys : None. 207: * Calls : None. 208: ******************************************************************************* 209: * 210: B35E 180B 0F 0909 [04] FLASHON MOVB #FLASHPAT1,FLASHPREV ; turn on the lights 211: B363 3D [05] RTS ; return to caller 212: * 213: ******************************************************************************* 214: * SUBROUTINE - FLASHOFF 215: * Description: Turns the flashing lights off 216: * Input : None. 217: * Output : None. 218: * Destroys : None. 219: * Calls : None. 220: ******************************************************************************* 221: * 222: B364 180B 00 0909 [04] FLASHOFF MOVB #$00,FLASHPREV ; turn off the lights 223: B369 3D [05] RTS ; return to caller 224: * 225: ******************************************************************************* 226: * SUBROUTINE - SIRENON 227: * Description: Turns the siren on 228: * Input : None. 229: * Output : None. 230: * Destroys : None. 231: * Calls : None. 232: ******************************************************************************* 233: * 234: B36A 180B 10 0088 [04] SIRENON MOVB #$10,TCTL1 ; turn on pin action for siren 235: B36F 4C 8C 40 [04] BSET TMSK1,#BIT6 ; turn on interrupts for siren 236: B372 1803 07D0 090A [05] MOVW #SIRENFREQ1,SIRENFREQ ; initialize the siren frequency 237: 238: B378 1803 FFF6 090C [05] MOVW #SIRENRATE1,SIRENTIMER ; start going up in frequency first 239: 240: B37E 3D [05] RTS ; return to caller 241: * 242: ******************************************************************************* 243: * SUBROUTINE - SIRENOFF 244: * Description: Turns the siren off 245: * Input : None. 246: * Output : None. 247: * Destroys : None. 248: * Calls : None. 249: ******************************************************************************* 250: * 251: B37F 4D 8C 40 [04] SIRENOFF BCLR TMSK1,#BIT6 ; turn off interrupts for siren 252: B382 180B 00 0088 [04] MOVB #$00,TCTL1 ; turn off pin action for siren 253: B387 1803 0000 090A [05] MOVW #$0000,SIRENFREQ ; clear siren frequency 254: B38D 3D [05] RTS ; return to caller 255: * 256: ******************************************************************************* 257: * INTERRUPT SERVICE ROUTINE - LEFTSTS 258: * Description: Handles the processing for a signal received from the left 259: * speed trap sensor. 260: * Input : None. 261: * Output : None. 262: * Destroys : None. 263: * Calls : None. 264: ******************************************************************************* 265: * 266: B38E 4F 8E 01 3F [04] LEFTSTS BRCLR TFLG1,BIT0,LEFTSTSX ; make sure we should be here 267: B392 180B 01 008E [04] MOVB #BIT0,TFLG1 ; clear the flag 268: 269: #ifdef __PRINTTIME_ 274: #endif 275: 276: B397 1804 0900 0902 [06] MOVW UPPERTIMER,LEFTSPDTIME ; save time beam was broken 277: B39D B6 0906 [03] LDAA SPDTIMEFLG ; load flags for broken beams 278: B3A0 8A 01 [01] ORAA #LSPDFLAG ; set the left flag 279: B3A2 81 03 [01] CMPA #BSPDFLAG ; have both beams been broken? 280: B3A4 27 05 [03] BEQ LEFTSTS1 ; yes, then go handle 281: B3A6 7A 0906 [03] STAA SPDTIMEFLG ; no, save flags for borken beams 282: B3A9 20 26 [03] BRA LEFTSTSX ; get out 283: 284: B3AB 180B 00 0906 [04] LEFTSTS1 MOVB #$00,SPDTIMEFLG ; clear flags for broken beams 285: B3B0 FC 0902 [03] LDD LEFTSPDTIME ; get the left beam broken time 286: B3B3 BC 0904 [03] CPD RIGHTSPDTIME ; did the timer roll over 287: B3B6 25 05 [03] BLO LEFTSTS2 ; yes, so calc speed differently 288: B3B8 B3 0904 [03] SUBD RIGHTSPDTIME ; calculate time difference 289: B3BB 20 06 [03] BRA LEFTSTS3 290: 291: B3BD FC 0904 [03] LEFTSTS2 LDD RIGHTSPDTIME ; load time right beam broken 292: B3C0 B3 0902 [03] SUBD LEFTSPDTIME ; calculate time difference 293: 294: B3C3 8C 001F [02] LEFTSTS3 CPD #SPDLIMIT ; was the object speeding? 295: B3C6 24 05 [03] BHS LEFTSTS4 ; no, so get out 296: 297: B3C8 180B 01 0907 [04] MOVB #LSPDFLAG,SPDCAUGHT ; yes, set flag for speeder caught 298: 299: B3CD 51 [01] LEFTSTS4 COMB ; 300: B3CE 7B 4000 [03] STAB SEG7PORT ; write speed to port 301: B3D1 0B [08] LEFTSTSX RTI ; return from interrupt 302: * 303: ******************************************************************************* 304: * INTERRUPT SERVICE ROUTINE - RIGHTSTS 305: * Description: Handles the processing for a signal received from the right 306: * speed trap sensor. 307: * Input : None. 308: * Output : None. 309: * Destroys : None. 310: * Calls : None. 311: ******************************************************************************* 312: * 313: B3D2 4F 8E 02 3F [04] RIGHTSTS BRCLR TFLG1,BIT1,RIGHTSTSX ; make sure we should be here 314: B3D6 180B 02 008E [04] MOVB #BIT1,TFLG1 ; clear the flag 315: 316: #ifdef __PRINTTIME_ 321: #endif 322: 323: B3DB 1804 0900 0904 [06] MOVW UPPERTIMER,RIGHTSPDTIME ; save time beam was broken 324: B3E1 B6 0906 [03] LDAA SPDTIMEFLG ; load flags for broken beams 325: B3E4 8A 02 [01] ORAA #RSPDFLAG ; set the right flag 326: B3E6 81 03 [01] CMPA #BSPDFLAG ; have both beams been broken? 327: B3E8 27 05 [03] BEQ RIGHTSTS1 ; yes, then go handle 328: B3EA 7A 0906 [03] STAA SPDTIMEFLG ; no, save flags for borken beams 329: B3ED 20 26 [03] BRA RIGHTSTSX ; get out 330: 331: B3EF 180B 00 0906 [04] RIGHTSTS1 MOVB #$00,SPDTIMEFLG ; clear flags for broken beams 332: B3F4 FC 0904 [03] LDD RIGHTSPDTIME ; get the right beam broken time 333: B3F7 BC 0902 [03] CPD LEFTSPDTIME ; did the timer roll over 334: B3FA 25 05 [03] BLO RIGHTSTS2 ; yes, so calc speed differently 335: B3FC B3 0902 [03] SUBD LEFTSPDTIME ; calculate time difference 336: B3FF 20 06 [03] BRA RIGHTSTS3 337: 338: B401 FC 0902 [03] RIGHTSTS2 LDD LEFTSPDTIME ; load time right beam broken 339: B404 B3 0904 [03] SUBD RIGHTSPDTIME ; calculate time difference 340: 341: B407 8C 001F [02] RIGHTSTS3 CPD #SPDLIMIT ; was the object speeding? 342: B40A 24 05 [03] BHS RIGHTSTS4 ; no, so get out 343: 344: B40C 180B 02 0907 [04] MOVB #RSPDFLAG,SPDCAUGHT ; yes, set flag for speeder caught (going right) 345: 346: B411 51 [01] RIGHTSTS4 COMB ; 347: B412 7B 4000 [03] STAB SEG7PORT ; write speed to port 348: B415 0B [08] RIGHTSTSX RTI ; return from interrupt 349: * 350: ******************************************************************************* 351: * INTERRUPT SERVICE ROUTINE - TIMEEXT 352: * Description: Increments the extended timer when a timer overflow occurs in 353: * TCNT (it is incremented every 8ms). 354: * Input : None. 355: * Output : None. 356: * Destroys : None. 357: * Calls : None. 358: ******************************************************************************* 359: * 360: B416 4F 8F 80 4B [04] TIMEEXT BRCLR TFLG2,BIT7,TIMEEXTX ; make sure we should be here 361: B41A 180B 80 008F [04] MOVB #BIT7,TFLG2 ; clear the flag 362: B41F FE 0900 [03] LDX UPPERTIMER ; get the timer extension 363: B422 08 [01] INX ; increment the timer extension 364: B423 7E 0900 [03] STX UPPERTIMER ; save the timer extension 365: 366: B426 FC 090A [03] LDD SIRENFREQ ; get previous siren frequency 367: B429 27 1E [03] BEQ TIMEEXT2 ; if siren off, keep it off 368: 369: B42B F3 090C [03] ADDD SIRENTIMER ; update siren frequency 370: B42E 7C 090A [03] STD SIRENFREQ ; save siren frequency 371: 372: B431 8C 07D0 [02] CPD #SIRENFREQ1 ; are we at low end of range 373: B434 25 08 [03] BLO TIMEEXT1 ; no, continue 374: B436 1803 FFF6 090C [05] MOVW #SIRENRATE1,SIRENTIMER ; yes, so start going back up 375: B43C 20 0B [03] BRA TIMEEXT2 ; continue 376: 377: B43E 8C 04B0 [02] TIMEEXT1 CPD #SIRENFREQ2 ; are we at high end of range 378: B441 22 06 [03] BHI TIMEEXT2 ; no, continue 379: B443 1803 000A 090C [05] MOVW #SIRENRATE2,SIRENTIMER ; yes, so start going down 380: 381: B449 F6 0909 [03] TIMEEXT2 LDAB FLASHPREV ; get previous status of lights 382: B44C 27 17 [03] BEQ TIMEEXTX ; if lights off, keep them off 383: 384: B44E B6 0908 [03] LDAA FLASHTIMER ; get the flashing lights timer 385: B451 42 [01] INCA ; increment the timer 386: B452 7A 0908 [03] STAA FLASHTIMER ; save flashing lights timer 387: B455 81 0F [01] CMPA #FLASHRATE ; do we need to change light status 388: B457 26 0C [03] BNE TIMEEXTX ; no, so get out 389: 390: B459 51 [01] COMB ; switch light pattern 391: B45A 7B 0909 [03] STAB FLASHPREV ; save new light pattern 392: B45D 7B 6000 [03] STAB LED1PORT ; turn on lights with new pattern 393: 394: B460 180B 00 0908 [04] MOVB #$00,FLASHTIMER ; reset flashing lights timer 395: 396: B465 0B [08] TIMEEXTX RTI ; return from interrupt 397: * 398: ******************************************************************************* 399: * INTERRUPT SERVICE ROUTINE - UPDMOTORS 400: * Description: Updates the speed of the motors on every RTI interrupt 401: * Input : None. 402: * Output : None. 403: * Destroys : None. 404: * Calls : None. 405: ******************************************************************************* 406: * 407: B466 4F 15 80 0B [04] UPDMOTORS BRCLR RTIFLG,BIT7,UPDMOTORSX ; make sure we should be here 408: B46A 180B 80 0015 [04] MOVB #BIT7,RTIFLG ; clear the flag 409: 410: B46F 16 B4DE [04] JSR LEFTMOTOR ; update speed on left motor 411: B472 16 B507 [04] JSR RIGHTMOTOR ; update speed on right motor 412: 413: B475 0B [08] UPDMOTORSX RTI ; return from interrupt 414: * 415: ******************************************************************************* 416: * INTERRUPT SERVICE ROUTINE - SIREN 417: * Description: Handles generating the frequency of the siren 418: * Input : None. 419: * Output : None. 420: * Destroys : None. 421: * Calls : None. 422: ******************************************************************************* 423: * 424: B476 4F 8E 40 0C [04] SIREN BRCLR TFLG1,BIT6,SIRENX ; make sure we should be here 425: B47A 180B 40 008E [04] MOVB #BIT6,TFLG1 ; clear the flag 426: 427: B47F DC 9C [03] LDD TC6 ; get previous interrupt time 428: B481 F3 090A [03] ADDD SIRENFREQ ; calc next time (set frequency of siren) 429: B484 5C 9C [02] STD TC6 ; set next interrupt time 430: 431: B486 0B [08] SIRENX RTI ; return from interrupt 432: * 433: ******************************************************************************* 203: #include "pwm.asm" 1: * Filename : PWM.ASM 2: * Programmer : Michael Hattermann 3: * Date : February 22, 2002 4: * Version : 1.0 5: * Description : This file contains the pulse width 6: * modulation functions for generating 7: * an output waveform. The following 8: * functions are available 9: * 10: * INITPWM - inits PWM system 11: * KILLPWM - shut down PWM system 12: * LEFTMOTOR - sets spd,dir for left motor 13: * RIGHTMOTOR - sets spd,dir for right motor 14: * CHNGSPEED - sets new speed for motors 15: * STEER - sets motors to perform known manuevers 16: * PULLOUT - move to begin chase 17: * 18: * 19: *#define __DEBUGPWM_ 1 20: 21: 22: #include "hc12.asm" 1: * Filename : HC12.H 2: * Programmer : Michael Hattermann 3: * Date : February 21, 2002 4: * Version : 1.0 5: * Description : This file contains equates for the HC12 6: * processor 7: * 8: * 9: ************************************************************ 10: * Register Equates 11: ************************************************************ 12: * 13: #ifndef __HC12_ASM_ 314: #endif 23: * 24: ************************************************************ 25: * PWM Equates 26: ************************************************************ 27: * 28: =00000000 PWM0 EQU 0 ; left motor 29: =00000001 PWM1 EQU 1 ; right motor 30: =00000002 PWM2 EQU 2 31: =00000003 PWM3 EQU 3 32: =00000004 PWM4 EQU 4 ; left direction 33: =00000005 PWM5 EQU 5 ; left brake 34: =00000006 PWM6 EQU 6 ; right direction 35: =00000007 PWM7 EQU 7 ; right brake 36: 37: =00000006 ACCELCONST EQU 6 ; acceleration constant 38: 39: =000000C8 FULLSPEED EQU 200 40: =000000AF _7_8_SPEED EQU 175 41: =00000096 _3_4_SPEED EQU 150 42: =0000007D _5_8_SPEED EQU 125 43: =00000064 HALFSPEED EQU 100 44: =0000004B _3_8_SPEED EQU 75 45: =00000032 _1_4_SPEED EQU 50 46: =00000019 _1_8_SPEED EQU 25 47: =00000000 STOPSPEED EQU 0 48: 49: =00000040 LEFTDIR EQU BIT6 50: =00000080 LEFTBRK EQU BIT7 51: =00000010 RIGHTDIR EQU BIT4 52: =00000020 RIGHTBRK EQU BIT5 53: 54: =00000000 GOFOWARD EQU 0 55: =00000001 GOBACK EQU 1 56: =00000002 HARDLEFT EQU 2 57: =00000003 SOFTLEFT EQU 3 58: =00000004 HARDRIGHT EQU 4 59: =00000005 SOFTRIGHT EQU 5 60: =00000006 BACKLEFT EQU 6 61: =00000007 BACKRIGHT EQU 7 62: =00000008 STOP EQU 8 63: 64: * 65: ************************************************************ 66: * PWM Debug Code 67: ************************************************************ 68: * 69: #ifdef __DEBUGPWM_ 134: #endif 135: * 136: ******************************************************************************* 137: * SUBROUTINE - INITPWM 138: * Description: Initializes the pulse width modulation system 139: * Input : None. 140: * Output : None. 141: * Destroys : None. 142: * Calls : None. 143: ******************************************************************************* 144: * 145: B487 180B 08 0040 [04] INITPWM MOVB #$08,PWCLK ; set prescaler bits, concatenate PWM channels 146: B48C 180B 33 0041 [04] MOVB #$33,PWPOL ; set clock source (S0,S1), polarity (high first) 147: B491 180B 63 0044 [04] MOVB #99,PWSCAL0 ; set S0 clock prescaler 148: B496 180B 00 0046 [04] MOVB #$00,PWSCAL1 ; init S1 clock prescaler 149: B49B 180B C8 004C [04] MOVB #200,PWPER0 ; set period of PWM0 to about 20ms 150: B4A0 180B C8 004D [04] MOVB #200,PWPER1 ; set period of PWM1 to about 20ms 151: B4A5 180B FF 004E [04] MOVB #$FF,PWPER2 ; init period of PWM2 152: B4AA 180B FF 004F [04] MOVB #$FF,PWPER3 ; init period of PWM3 153: B4AF 180B 00 0050 [04] MOVB #$00,PWDTY0 ; init duty cycle to 0% 154: B4B4 180B 00 0051 [04] MOVB #$00,PWDTY1 ; init duty cycle to 0% 155: B4B9 180B 00 0052 [04] MOVB #$00,PWDTY2 ; init duty cycle to 0% 156: B4BE 180B 00 0053 [04] MOVB #$00,PWDTY3 ; init duty cycle to 0% 157: B4C3 180B 00 0054 [04] MOVB #$00,PWCTL ; init PWM to run normally, left aligned 158: B4C8 180B 50 0057 [04] MOVB #$50,DDRP ; configure unused bits to be outputs (motor controls) 159: B4CD 180B 50 0056 [04] MOVB #$50,PORTP ; set direction to foward, brake to off 160: B4D2 180B 03 0042 [04] MOVB #$03,PWEN ; enable the PWM channels 161: B4D7 3D [05] RTS ; return to caller 162: * 163: ******************************************************************************* 164: * SUBROUTINE - KILLPWM 165: * Description: Shuts down the pulse width modulation system 166: * Input : None. 167: * Output : None. 168: * Destroys : None. 169: * Calls : None. 170: ******************************************************************************* 171: * 172: B4D8 180B 00 0042 [04] KILLPWM MOVB #$00,PWEN ; turn off PWM channels 173: B4DD 3D [05] RTS ; return to caller 174: * 175: ******************************************************************************* 176: * SUBROUTINE - LEFTMOTOR 177: * Description: Sets the left motor to the speed specified. Valid 178: * speed values range from -100 to 100, where 0 to -100 is back- 179: * wards speed and 0 to 100 is foward speed. 180: * 181: * Input : None. 182: * Output : None. 183: * Destroys : None. 184: * Calls : None. 185: ******************************************************************************* 186: * 187: B4DE 34 [02] LEFTMOTOR PSHX ; save reg X 188: B4DF 35 [02] PSHY ; save reg Y 189: B4E0 3B [02] PSHD ; save reg D 190: 191: B4E1 FC 090E [03] LDD OLEFTSPD ; load the old speed 192: B4E4 CD 0006 [02] LDY #ACCELCONST ; get the acceleration constant 193: B4E7 1813 [03] EMULS ; multiply old speed by acceleration constant 194: B4E9 F3 0910 [03] ADDD NLEFTSPD ; add new speed to result 195: B4EC CE 0007 [02] LDX #(ACCELCONST+1) ; load divisor 196: B4EF 1815 [0C] IDIVS ; calc new speed 197: B4F1 B7C5 [01] XGDX ; put new speed in reg D 198: B4F3 7C 090E [03] STD OLEFTSPD ; save new speed as old speed 199: B4F6 2D 05 [03] BLT LEFTMOTOR1 ; if new speed is negative, branch 200: B4F8 4C 56 40 [04] BSET PORTP,LEFTDIR ; set direction to foward 201: B4FB 20 04 [03] BRA LEFTMOTOR2 ; continue 202: B4FD 4D 56 40 [04] LEFTMOTOR1 BCLR PORTP,LEFTDIR ; set direction to reverse 203: B500 50 [01] NEGB ; take abs value of speed 204: B501 5B 51 [02] LEFTMOTOR2 STAB PWDTY1 ; set the new speed for the left motor 205: 206: B503 3A [03] PULD ; restore reg D 207: B504 31 [03] PULY ; restore reg Y 208: B505 30 [03] PULX ; restore reg X 209: B506 3D [05] LEFTMOTORX RTS ; return to caller 210: * 211: ******************************************************************************* 212: * SUBROUTINE - RIGHTMOTOR 213: * Description: Sets the right motor to the speed specified. Valid 214: * speed values range from -100 to 100, where 0 to -100 is back- 215: * wards speed and 0 to 100 is foward speed. 216: * 217: * Input : None. 218: * Output : None. 219: * Destroys : None. 220: * Calls : None. 221: ******************************************************************************* 222: * 223: B507 34 [02] RIGHTMOTOR PSHX ; save reg X 224: B508 35 [02] PSHY ; save reg Y 225: B509 3B [02] PSHD ; save reg D 226: 227: B50A FC 0912 [03] LDD ORIGHTSPD ; load the old speed 228: B50D CD 0006 [02] LDY #ACCELCONST ; get the acceleration constant 229: B510 1813 [03] EMULS ; multiply old speed by acceleration constant 230: B512 F3 0914 [03] ADDD NRIGHTSPD ; add new speed to result 231: B515 CE 0007 [02] LDX #(ACCELCONST+1) ; load divisor 232: B518 1815 [0C] IDIVS ; calc new speed 233: B51A B7C5 [01] XGDX ; put new speed in reg D 234: B51C 7C 0912 [03] STD ORIGHTSPD ; save new speed as old speed 235: B51F 2D 05 [03] BLT RIGHTMOTOR1 ; if new speed is negative, branch 236: B521 4C 56 10 [04] BSET PORTP,RIGHTDIR ; set direction to foward 237: B524 20 04 [03] BRA RIGHTMOTOR2 ; continue 238: B526 4D 56 10 [04] RIGHTMOTOR1 BCLR PORTP,RIGHTDIR ; set direction to reverse 239: B529 50 [01] NEGB ; take abs value of speed 240: B52A 5B 50 [02] RIGHTMOTOR2 STAB PWDTY0 ; set the new speed for the right motor 241: 242: B52C 3A [03] PULD ; restore reg D 243: B52D 31 [03] PULY ; restore reg Y 244: B52E 30 [03] PULX ; restore reg X 245: B52F 3D [05] RIGHTMOTORX RTS ; return to caller 246: * 247: ******************************************************************************* 248: * SUBROUTINE - CHNGSPEED 249: * Description: Changes the maximum speed for either motor to the speed passed 250: * in register D. 251: * Input : Speed(reg D). 252: * Output : None. 253: * Destroys : None. 254: * Calls : None. 255: ******************************************************************************* 256: * 257: B530 7C 0916 [03] CHNGSPEED STD MAXFOWARDSPD ; save max speed 258: B533 7C 0918 [03] STD MAXBACKSPD ; save max speed to backward 259: B536 71 0918 [04] COM MAXBACKSPD ; convert to negative speed 260: B539 70 0919 [04] NEG MAXBACKSPD+1 ; 261: B53C 3D [05] RTS ; return to caller 262: * 263: ******************************************************************************* 264: * SUBROUTINE - STEER 265: * Description: Sets up the left and right motors to perform the specified 266: * manuever (reg X). The value of manuever must be one of the 267: * following: 268: * 269: * GOFOWARD 270: * GOBACK 271: * HARDLEFT 272: * SOFTLEFT 273: * HARDRIGHT 274: * SOFTRIGHT 275: * BACKLEFT 276: * BACKRIGHT 277: * STOP 278: * 279: * Input : Manuever(reg X). 280: * Output : None. 281: * Destroys : None. 282: * Calls : LEFTMOTOR,RIGHTMOTOR. 283: ******************************************************************************* 284: * 285: B53D 3B [02] STEER PSHD ; save register D 286: B53E 34 [02] PSHX ; save register X 287: 288: B53F 7E 0922 [03] STX CURRMAN ; save manuever 289: B542 04 65 0B [03] TBNE X,STEER1 ; if not FOWARD, continue 290: B545 FC 0916 [03] LDD MAXFOWARDSPD ; load maximum foward speed 291: B548 7C 0914 [03] STD NRIGHTSPD ; right motor full foward 292: B54B 7C 0910 [03] STD NLEFTSPD ; left motor foward full 293: B54E 20 7B [03] BRA STEERX ; get out 294: 295: B550 04 25 0B [03] STEER1 DBNE X,STEER2 ; if not BACKWARD, continue 296: B553 FC 0918 [03] LDD MAXBACKSPD ; load maximum back speed 297: B556 7C 0914 [03] STD NRIGHTSPD ; right motor back full 298: B559 7C 0910 [03] STD NLEFTSPD ; left motor back full 299: B55C 20 6D [03] BRA STEERX ; get out 300: 301: B55E 04 25 0E [03] STEER2 DBNE X,STEER3 ; if not HARD LEFT, continue 302: B561 FC 0916 [03] LDD MAXFOWARDSPD ; load maximum foward speed 303: B564 7C 0914 [03] STD NRIGHTSPD ; right motor foward full 304: B567 FC 0918 [03] LDD MAXBACKSPD ; load maximum back speed 305: B56A 7C 0910 [03] STD NLEFTSPD ; left motor backward full 306: B56D 20 5C [03] BRA STEERX ; get out 307: 308: B56F 04 25 0C [03] STEER3 DBNE X,STEER4 ; if not SOFT LEFT, continue 309: B572 FC 0916 [03] LDD MAXFOWARDSPD ; load maximum foward speed 310: B575 7C 0914 [03] STD NRIGHTSPD ; right motor foward full 311: B578 49 [01] LSRD ; set left motor speed 312: B579 7C 0910 [03] STD NLEFTSPD ; left motor foward half 313: B57C 20 4D [03] BRA STEERX ; get out 314: 315: B57E 04 25 0E [03] STEER4 DBNE X,STEER5 ; if not HARD RIGHT, continue 316: B581 FC 0916 [03] LDD MAXFOWARDSPD ; load maximum foward speed 317: B584 7C 0910 [03] STD NLEFTSPD ; left motor foward full 318: B587 FC 0918 [03] LDD MAXBACKSPD ; load maximum back speed 319: B58A 7C 0914 [03] STD NRIGHTSPD ; right motor back full 320: B58D 20 3C [03] BRA STEERX ; get out 321: 322: B58F 04 25 0C [03] STEER5 DBNE X,STEER6 ; if not SOFT RIGHT, continue 323: B592 FC 0916 [03] LDD MAXFOWARDSPD ; load maximum foward speed 324: B595 7C 0910 [03] STD NLEFTSPD ; left motor foward full 325: B598 49 [01] LSRD ; set right motor speed 326: B599 7C 0914 [03] STD NRIGHTSPD ; right motor foward half 327: B59C 20 2D [03] BRA STEERX ; get out 328: 329: 330: B59E 04 25 0C [03] STEER6 DBNE X,STEER7 ; if not BACK LEFT, continue 331: B5A1 FC 0918 [03] LDD MAXBACKSPD ; load maximum foward speed 332: B5A4 7C 0914 [03] STD NRIGHTSPD ; right motor back full 333: B5A7 49 [01] LSRD ; set left motor speed 334: B5A8 7C 0910 [03] STD NLEFTSPD ; left motor foward half 335: B5AB 20 1E [03] BRA STEERX ; get out 336: 337: B5AD 04 25 0C [03] STEER7 DBNE X,STEER8 ; if not BACK RIGHT, continue 338: B5B0 FC 0918 [03] LDD MAXBACKSPD ; load maximum foward speed 339: B5B3 7C 0910 [03] STD NLEFTSPD ; left motor back full 340: B5B6 49 [01] LSRD ; set left motor speed 341: B5B7 7C 0914 [03] STD NRIGHTSPD ; right motor foward half 342: B5BA 20 0F [03] BRA STEERX ; get out 343: 344: B5BC 04 25 0C [03] STEER8 DBNE X,STEERX ; if not STOP, get out 345: B5BF 1803 0000 0910 [05] MOVW #$0000,NLEFTSPD ; stop left motor 346: B5C5 1803 0000 0914 [05] MOVW #$0000,NRIGHTSPD ; stop right motor 347: 348: B5CB 30 [03] STEERX PULX ; restore register X 349: B5CC 3A [03] PULD ; restore register D 350: B5CD 3D [05] RTS ; return to caller 351: * 352: ******************************************************************************* 353: * SUBROUTINE - PULLOUT 354: * Description: Performs pre-programmed manuever to pull out onto the road and 355: * turn toward moving objext to begin chasing 356: * Input : None. 357: * Output : None. 358: * Destroys : None. 359: * Calls : None. 360: ******************************************************************************* 361: * 362: B5CE 3B [02] PULLOUT PSHD ; save reg D 363: B5CF 34 [02] PSHX ; save reg X 364: 365: B5D0 CC 00C8 [02] LDD #FULLSPEED ; set motor speed 366: B5D3 16 B530 [04] JSR CHNGSPEED ; 367: 368: B5D6 CE 0000 [02] LDX #GOFOWARD ; pull foward 369: B5D9 16 B53D [04] JSR STEER ; 370: B5DC CE 003C [02] LDX #60 ; for a little bit 371: B5DF 16 B171 [04] JSR WAIT ; 372: 373: B5E2 B6 0907 [03] LDAA SPDCAUGHT ; get direction of speeder 374: B5E5 81 01 [01] CMPA #LSPDFLAG ; was the speeder going left? 375: B5E7 27 0E [03] BEQ PULLOUTL ; yes, so go pull out left 376: B5E9 PULLOUTR 377: B5E9 CE 0005 [02] LDX #SOFTRIGHT ; no, then pull out right 378: B5EC 16 B53D [04] JSR STEER ; 379: B5EF CE 0271 [02] LDX #625 ; wait for manuever 380: B5F2 16 B171 [04] JSR WAIT ; 381: B5F5 20 0C [03] BRA PULLOUTX ; get out 382: B5F7 PULLOUTL 383: B5F7 CE 0003 [02] LDX #SOFTLEFT ; turn left 384: B5FA 16 B53D [04] JSR STEER ; 385: B5FD CE 0258 [02] LDX #600 ; wait for manuever 386: B600 16 B171 [04] JSR WAIT ; 387: 388: B603 30 [03] PULLOUTX PULX ; restore reg X 389: B604 3A [03] PULD ; restore reg D 390: B605 3D [05] RTS ; return to caller 391: * 392: ******************************************************************************* 393: 204: #include "objavoid2.asm" 1: * Filename : OBJAVOID.ASM 2: * Programmer : Michael Hattermann 3: * Date : February 4, 2002 4: * Version : 1.0 5: * Description : This file contains the code for 6: * object avoidance. The following 7: * functions are available: 8: * 9: * OBJAVOID - reads IR and avoids obstacles 10: * GETVALUES - reads IR values from analog port 11: * CONVREACT - converts IR value to a reaction 12: * LEFTRIGHT - decided to turn left,right,or random 13: * BACKUP - backs robot up and turns it 14: * HARD - turns robot hard in a direction 15: * SOFT - turns robot soft in a direction 16: * 17: * 18: *#define __DEBUGOBJAVOID2_ 1 19: *#define __PRINTOBJAVOID2_ 1 20: #include "hc12.asm" 1: * Filename : HC12.H 2: * Programmer : Michael Hattermann 3: * Date : February 21, 2002 4: * Version : 1.0 5: * Description : This file contains equates for the HC12 6: * processor 7: * 8: * 9: ************************************************************ 10: * Register Equates 11: ************************************************************ 12: * 13: #ifndef __HC12_ASM_ 314: #endif 21: 22: 23: * 24: ************************************************************ 25: * Object Avoidance Equates 26: ************************************************************ 27: * 28: =00000010 LREQUAL EQU $10 ; left,right IR values equality threshold 29: =00000096 BACKUPTIME EQU 150 ; # ms to backup 30: 31: * 32: ************************************************************ 33: * OBJAVOID2 Debug Code 34: ************************************************************ 35: * 36: #ifdef __DEBUGOBJAVOID2_ 62: #endif 63: 64: #ifdef __PRINTOBJAVOID2_ 89: #endif 90: 91: * Reaction table for values for center IR 92: ************************************************************ 93: B606 00 CNTRTBL DC.B NOACTION ; $00-$07 94: B607 00 DC.B NOACTION ; $08-$0F 95: B608 00 DC.B NOACTION ; $10-$17 96: B609 00 DC.B NOACTION ; $18-$1F 97: B60A 03 DC.B REACTSOFT ; $20-$27 98: B60B 03 DC.B REACTSOFT ; $28-$2F 99: B60C 03 DC.B REACTSOFT ; $30-$37 100: B60D 03 DC.B REACTSOFT ; $38-$3F 101: B60E 03 DC.B REACTSOFT ; $40-$47 102: B60F 02 DC.B REACTHARD ; $48-$4F 103: B610 02 DC.B REACTHARD ; $50-$57 104: B611 02 DC.B REACTHARD ; $58-$5F 105: B612 02 DC.B REACTHARD ; $60-$67 106: B613 02 DC.B REACTHARD ; $68-$6F 107: B614 01 DC.B REACTBACK ; $70-$77 108: B615 01 DC.B REACTBACK ; $78-$7F 109: B616 01 DC.B REACTBACK ; $80-$87 110: B617 01 DC.B REACTBACK ; $88-$8F 111: B618 01 DC.B REACTBACK ; $90-$97 112: B619 01 DC.B REACTBACK ; $98-$9F 113: 114: * Reaction table for values for left,right IR 115: ************************************************************ 116: B61A 00 LEFTTBL DC.B NOACTION ; $00-$07 117: B61B 00 DC.B NOACTION ; $08-$0F 118: B61C 00 DC.B NOACTION ; $10-$17 119: B61D 00 DC.B NOACTION ; $18-$1F 120: B61E 03 DC.B REACTSOFT ; $20-$27 121: B61F 03 DC.B REACTSOFT ; $28-$2F 122: B620 03 DC.B REACTSOFT ; $30-$37 123: B621 03 DC.B REACTSOFT ; $38-$3F 124: B622 02 DC.B REACTHARD ; $40-$47 125: B623 02 DC.B REACTHARD ; $48-$4F 126: B624 02 DC.B REACTHARD ; $50-$57 127: B625 02 DC.B REACTHARD ; $58-$5F 128: B626 02 DC.B REACTHARD ; $60-$67 129: B627 02 DC.B REACTHARD ; $68-$6F 130: B628 01 DC.B REACTBACK ; $70-$77 131: B629 01 DC.B REACTBACK ; $78-$7F 132: B62A 01 DC.B REACTBACK ; $80-$87 133: B62B 01 DC.B REACTBACK ; $88-$8F 134: B62C 01 DC.B REACTBACK ; $90-$97 135: B62D 01 DC.B REACTBACK ; $98-$9F 136: 137: * Reaction table for values for left,right IR 138: ************************************************************ 139: B62E 00 RIGHTTBL DC.B NOACTION ; $00-$07 140: B62F 00 DC.B NOACTION ; $08-$0F 141: B630 00 DC.B NOACTION ; $10-$17 142: B631 00 DC.B NOACTION ; $18-$1F 143: B632 03 DC.B REACTSOFT ; $20-$27 144: B633 03 DC.B REACTSOFT ; $28-$2F 145: B634 03 DC.B REACTSOFT ; $30-$37 146: B635 03 DC.B REACTSOFT ; $38-$3F 147: B636 02 DC.B REACTHARD ; $40-$47 148: B637 02 DC.B REACTHARD ; $48-$4F 149: B638 02 DC.B REACTHARD ; $50-$57 150: B639 02 DC.B REACTHARD ; $58-$5F 151: B63A 02 DC.B REACTHARD ; $60-$67 152: B63B 02 DC.B REACTHARD ; $68-$6F 153: B63C 01 DC.B REACTBACK ; $70-$77 154: B63D 01 DC.B REACTBACK ; $78-$7F 155: B63E 01 DC.B REACTBACK ; $80-$87 156: B63F 01 DC.B REACTBACK ; $88-$8F 157: B640 01 DC.B REACTBACK ; $90-$97 158: B641 01 DC.B REACTBACK ; $98-$9F 159: 160: * 161: ******************************************************************************* 162: * SUBROUTINE - OBJAVOID 163: * Description: Performs an obstacle avoidance behavior by reading the values 164: * from the IR and bump sensors and moving the robot accordingly. 165: * Input : None. 166: * Output : None. 167: * Destroys : None. 168: * Calls : GETVALUES,CONVREACT. 169: ******************************************************************************* 170: * 171: B642 3B [02] OBJAVOID PSHD ; save reg D 172: B643 34 [02] PSHX ; save reg X 173: 174: B644 16 B1AE [04] JSR BUMPED ; check to see if we bumped something 175: B647 81 47 [01] CMPA #FRONTREAR ; did we bump something in the front? 176: B649 25 03 [03] BLO OBJAVOIDB ; no bump, check the IR 177: B64B 06 B729 [03] JMP BACKUP ; we bumped something, backup 178: 179: B64E 16 B6B3 [04] OBJAVOIDB JSR GETVALUES ; get the IR readings 180: B651 16 B6CE [04] JSR CONVREACT ; convert readings to reactions 181: 182: B654 C6 01 [01] LDAB #REACTBACK ; get code for back up reaction 183: B656 F1 091F [03] CMPB CREACT ; does center say backup 184: B659 26 03 [03] BNE OBJAVOID1 ; no, continue checking 185: B65B 06 B729 [03] JMP BACKUP ; yes, backup 186: 187: B65E F1 091B [03] OBJAVOID1 CMPB LREACT ; does left say backup 188: B661 26 03 [03] BNE OBJAVOID2 ; no, continue checking 189: B663 06 B729 [03] JMP BACKUP ; yes, backup 190: 191: B666 F1 091D [03] OBJAVOID2 CMPB RREACT ; does right say backup 192: B669 26 03 [03] BNE OBJAVOID3 ; no, continue checking 193: B66B 06 B729 [03] JMP BACKUP ; yes, backup 194: 195: B66E C6 02 [01] OBJAVOID3 LDAB #REACTHARD ; get code for hard turn reaction 196: B670 F1 091F [03] CMPB CREACT ; does center say backup 197: B673 26 03 [03] BNE OBJAVOID4 ; no, continue checking 198: B675 06 B755 [03] JMP HARD ; yes, turn hard 199: 200: B678 F1 091B [03] OBJAVOID4 CMPB LREACT ; does left say turn hard 201: B67B 26 03 [03] BNE OBJAVOID5 ; no, continue checking 202: B67D 06 B755 [03] JMP HARD ; yes, turn hard 203: 204: B680 F1 091D [03] OBJAVOID5 CMPB RREACT ; does right say turn hard 205: B683 26 03 [03] BNE OBJAVOID6 ; no, continue checking 206: B685 06 B755 [03] JMP HARD ; yes, turn hard 207: 208: B688 C6 03 [01] OBJAVOID6 LDAB #REACTSOFT ; get code for hard turn reaction 209: B68A F1 091F [03] CMPB CREACT ; does center say backup 210: B68D 26 03 [03] BNE OBJAVOID7 ; no, continue checking 211: B68F 06 B77D [03] JMP SOFT ; yes, turn soft 212: 213: B692 F1 091B [03] OBJAVOID7 CMPB LREACT ; does left say turn hard 214: B695 26 03 [03] BNE OBJAVOID8 ; no, continue checking 215: B697 06 B77D [03] JMP SOFT ; yes, turn soft 216: 217: B69A F1 091D [03] OBJAVOID8 CMPB RREACT ; does right say turn hard 218: B69D 26 03 [03] BNE OBJAVOID9 ; no, continue checking 219: B69F 06 B77D [03] JMP SOFT ; yes, turn hard 220: 221: B6A2 OBJAVOID9 222: #ifdef __PRINTOBJAVOID2_ 227: #endif 228: 229: B6A2 CE 0000 [02] OBJAVOID10 LDX #GOFOWARD ; no obstacles, go foward at set speed 230: B6A5 16 B53D [04] JSR STEER ; 231: B6A8 79 0920 [03] CLR PREVRAND ; clear previous random direction 232: B6AB 180B 00 0921 [04] MOVB #NOACTION,PREVREACT ; save this reaction 233: 234: B6B0 30 [03] OBJAVOIDX PULX ; restore reg X 235: B6B1 3A [03] PULD ; restore reg D 236: B6B2 3D [05] RTS ; return to caller 237: * 238: ******************************************************************************* 239: * SUBROUTINE - GETVALUES 240: * Description: Gets the IR values for the left,center,and right channels 241: * Input : None. 242: * Output : RVALUE,LVALUE,CVALUE. 243: * Destroys : RVALUE,LVALUE,CVALUE. 244: * Calls : ANALOG. 245: ******************************************************************************* 246: * 247: B6B3 36 [02] GETVALUES PSHA ; save reg A 248: B6B4 86 01 [01] LDAA #RIGHTIR ; read right IR value 249: B6B6 16 B26C [04] JSR ANALOG ; 250: B6B9 7A 091C [03] STAA RVALUE ; save right IR value 251: B6BC 86 00 [01] LDAA #LEFTIR ; read left IR value 252: B6BE 16 B26C [04] JSR ANALOG ; 253: B6C1 7A 091A [03] STAA LVALUE ; save left IR value 254: B6C4 86 02 [01] LDAA #CENTERIR ; read center IR value 255: B6C6 16 B26C [04] JSR ANALOG ; 256: B6C9 7A 091E [03] STAA CVALUE ; save center IR value 257: 258: #ifdef __PRINTOBJAVOID2_ 275: #endif 276: 277: B6CC 32 [03] PULA ; restore reg A 278: B6CD 3D [05] RTS ; return to caller 279: * 280: ******************************************************************************* 281: * SUBROUTINE - CONVREACT 282: * Description: Converts IR readings to reaction values using lookup tables 283: * Input : RVALUE,LVALUE,CVALUE. 284: * Output : CREACT,LREACT,RREACT. 285: * Destroys : CREACT,LREACT,RREACT. 286: * Calls : None. 287: ******************************************************************************* 288: * 289: B6CE 34 [02] CONVREACT PSHX ; save register X 290: B6CF CE B606 [02] LDX #CNTRTBL ; load address of lookup table 291: B6D2 F6 091E [03] LDAB CVALUE ; get center value 292: B6D5 54 [01] LSRB ; convert center value 293: B6D6 54 [01] LSRB ; to table lookup 294: B6D7 54 [01] LSRB ; value 295: B6D8 180D E5 091F [05] MOVB B,X,CREACT ; lookup reaction for center 296: 297: B6DD CE B61A [02] LDX #LEFTTBL ; load address of lookup table 298: B6E0 F6 091A [03] LDAB LVALUE ; get left value 299: B6E3 54 [01] LSRB ; convert left value 300: B6E4 54 [01] LSRB ; to table lookup 301: B6E5 54 [01] LSRB ; value 302: B6E6 180D E5 091B [05] MOVB B,X,LREACT ; lookup reaction for left channel 303: 304: B6EB CE B62E [02] LDX #RIGHTTBL ; load address of lookup table 305: B6EE F6 091C [03] LDAB RVALUE ; get right value 306: B6F1 54 [01] LSRB ; convert right value 307: B6F2 54 [01] LSRB ; to table lookup 308: B6F3 54 [01] LSRB ; value 309: B6F4 180D E5 091D [05] MOVB B,X,RREACT ; lookup reaction for right channel 310: 311: #ifdef __PRINTOBJAVOID2_ 328: #endif 329: B6F9 30 [03] PULX ; restore register X 330: B6FA 3D [05] RTS ; return to caller 331: * 332: ******************************************************************************* 333: * SUBROUTINE - LEFTRIGHT 334: * Description: Decides if we should turn left or right based on IR sensor 335: * readings. It will compare left and right values, and if they 336: * differ by more than some threshold, this decides the turn 337: * direction. Otherwise, turn direction is random. Return values: 338: * REACTLEFT - Turn left 339: * REACTRIGHT - Turn right 340: * Input : LVALUE,RVALUE. 341: * Output : Reg B has direction. 342: * Destroys : Reg B, PREVRAND. 343: * Calls : None. 344: ******************************************************************************* 345: * 346: B6FB 36 [02] LEFTRIGHT PSHA ; save reg A 347: 348: B6FC B6 091A [03] LDAA LVALUE ; get the left value 349: B6FF F6 091C [03] LDAB RVALUE ; get the right value 350: B702 1816 [02] SBA ; compare the values 351: B704 81 10 [01] CMPA #LREQUAL ; if left > right by the threshold 352: B706 2C 13 [03] BGE LEFTRIGHT1 ; turn right 353: B708 81 F0 [01] CMPA #(-LREQUAL) ; if left < right by the threshold 354: B70A 2F 16 [03] BLE LEFTRIGHT2 ; turn left 355: 356: B70C F7 0920 [04] TST PREVRAND ; do we have a previous direction 357: B70F 27 05 [03] BEQ LEFTRIGHTR ; no, so go generate a direction 358: B711 F6 0920 [03] LDAB PREVRAND ; yes, so use previous direction 359: B714 20 11 [03] BRA LEFTRIGHTX ; get out 360: 361: B716 D6 85 [03] LEFTRIGHTR LDAB TCNTL ; otherwise, get lower half of timer 362: B718 54 [01] LSRB ; check lowest bit 363: B719 25 07 [03] BCS LEFTRIGHT2 ; go left if set, right if clear 364: B71B C6 02 [01] LEFTRIGHT1 LDAB #REACTRIGHT ; turn right 365: B71D 7B 0920 [03] STAB PREVRAND ; save previous direction 366: B720 20 05 [03] BRA LEFTRIGHTX ; get out 367: B722 C6 01 [01] LEFTRIGHT2 LDAB #REACTLEFT ; turn left 368: B724 7B 0920 [03] STAB PREVRAND ; save previous direction 369: B727 32 [03] LEFTRIGHTX PULA ; restore reg A 370: B728 3D [05] RTS ; return to caller 371: * 372: ******************************************************************************* 373: * SUBROUTINE - BACKUP 374: * Description: Backs up and turns a random direction (left or right). 375: * Input : None. 376: * Output : None. 377: * Destroys : Reg B,Reg X,PREVRAND. 378: * Calls : STEER,WAIT. 379: ******************************************************************************* 380: * 381: B729 BACKUP 382: B729 180B 01 0921 [04] MOVB #REACTBACK,PREVREACT ; save this reaction 383: B72E 79 0920 [03] CLR PREVRAND ; clear previous random direction 384: 385: #ifdef __PRINTOBJAVOID2_ 390: #endif 391: 392: B731 16 B6FB [04] BACKUP1 JSR LEFTRIGHT ; do we have a preference left or right 393: B734 C1 01 [01] CMPB #REACTLEFT ; if we need to go left, go left 394: B736 27 08 [03] BEQ BACKUPL ; go left 395: 396: B738 CE 0007 [02] LDX #BACKRIGHT ; go hard right 397: B73B 16 B53D [04] JSR STEER ; 398: B73E 20 06 [03] BRA BACKUPX ; get out 399: 400: B740 CE 0006 [02] BACKUPL LDX #BACKLEFT ; go hard left 401: B743 16 B53D [04] JSR STEER ; 402: 403: B746 CE 0001 [02] BACKUPX LDX #GOBACK ; go backward 404: B749 16 B53D [04] JSR STEER ; 405: B74C CE 0096 [02] LDX #BACKUPTIME ; go back for set amount of time 406: B74F 16 B171 [04] JSR WAIT ; 407: B752 06 B6B0 [03] JMP OBJAVOIDX ; get out 408: * 409: ******************************************************************************* 410: * SUBROUTINE - HARD 411: * Description: Turns hard in a random direction (left or right). 412: * Input : None. 413: * Output : None. 414: * Destroys : Reg B,Reg X. 415: * Calls : STEER. 416: ******************************************************************************* 417: * 418: B755 HARD 419: B755 F6 0921 [03] LDAB PREVREACT ; get previous reaction 420: B758 C1 02 [01] CMPB #REACTHARD ; was it react hard? 421: B75A 27 08 [03] BEQ HARD1 ; yes, so continue 422: B75C 79 0920 [03] CLR PREVRAND ; no, clear previous turn direction 423: B75F 180B 02 0921 [04] MOVB #REACTHARD,PREVREACT ; save this reaction 424: B764 HARD1 425: B764 16 B6FB [04] JSR LEFTRIGHT ; do we have a preference left or right 426: B767 C1 01 [01] CMPB #REACTLEFT ; if we need to go left, go left 427: B769 27 09 [03] BEQ HARDL ; go left 428: 429: #ifdef __PRINTOBJAVOID2_ 434: #endif 435: 436: B76B CE 0004 [02] HARDR LDX #HARDRIGHT ; go hard right 437: B76E 16 B53D [04] JSR STEER ; 438: B771 06 B6B0 [03] JMP OBJAVOIDX ; get out 439: 440: B774 HARDL 441: 442: #ifdef __PRINTOBJAVOID2_ 447: #endif 448: 449: B774 CE 0002 [02] LDX #HARDLEFT ; go hard left 450: B777 16 B53D [04] JSR STEER ; 451: B77A 06 B6B0 [03] JMP OBJAVOIDX ; get out 452: * 453: ******************************************************************************* 454: * SUBROUTINE - SOFT 455: * Description: Turns soft in a random direction (left or right). 456: * Input : None. 457: * Output : None. 458: * Destroys : Reg B,Reg X. 459: * Calls : STEER,WAIT. 460: ******************************************************************************* 461: * 462: B77D SOFT 463: B77D F6 0921 [03] LDAB PREVREACT ; get previous reaction 464: B780 C1 03 [01] CMPB #REACTSOFT ; was it react soft? 465: B782 27 08 [03] BEQ SOFT1 ; yes, so continue 466: B784 79 0920 [03] CLR PREVRAND ; no, clear previous turn direction 467: B787 180B 03 0921 [04] MOVB #REACTSOFT,PREVREACT ; save this reaction 468: B78C SOFT1 469: B78C 16 B6FB [04] JSR LEFTRIGHT ; do we have a preference left or right 470: B78F C1 01 [01] CMPB #REACTLEFT ; if we need to go left, go left 471: B791 27 09 [03] BEQ SOFTL ; go left 472: 473: #ifdef __PRINTOBJAVOID2_ 478: #endif 479: 480: B793 CE 0005 [02] SOFTR LDX #SOFTRIGHT ; go soft right 481: B796 16 B53D [04] JSR STEER ; 482: B799 06 B6B0 [03] JMP OBJAVOIDX ; get out 483: 484: B79C SOFTL 485: #ifdef __PRINTOBJAVOID2_ 490: #endif 491: 492: B79C CE 0003 [02] LDX #SOFTLEFT ; go soft left 493: B79F 16 B53D [04] JSR STEER ; 494: B7A2 06 B6B0 [03] JMP OBJAVOIDX ; get out 495: * 496: ******************************************************************************* 205: #include "follow.asm" 1: * Filename : FOLLOW.ASM 2: * Programmer : Michael Hattermann 3: * Date : March 31, 2002 4: * Version : 1.0 5: * Description : This file contains the code for 6: * object following. The following 7: * functions are available: 8: * 9: * 10: * 11: *#define __DEBUGFOLLOW_ 1 12: *#define __PRINTFOLLOW_ 1 13: 14: #include "hc12.asm" 1: * Filename : HC12.H 2: * Programmer : Michael Hattermann 3: * Date : February 21, 2002 4: * Version : 1.0 5: * Description : This file contains equates for the HC12 6: * processor 7: * 8: * 9: ************************************************************ 10: * Register Equates 11: ************************************************************ 12: * 13: #ifndef __HC12_ASM_ 314: #endif 15: 16: * 17: ************************************************************ 18: * Object Following Equates 19: ************************************************************ 20: * 21: =00000055 MINFOLLOW EQU $55 ; minimum reading on sensor to execute following 22: =00000064 OBJAVOIDSPD EQU HALFSPEED ; speed to perform object avoidance at 23: =00000064 MINFOLLOWSPD EQU HALFSPEED ; minimum speed to follow at 24: =000000C8 MAXFOLLOWSPD EQU FULLSPEED ; maximum speed to follow at 25: =0000007D FOLLOWTURNSPD EQU _5_8_SPEED ; max speed to perform a hard turn at 26: =00000014 NUMAVGCNT EQU 20 ; number of sensor values to average 27: =00000001 SPDACCEL EQU 1 ; desired amount of acceleration 28: =FFFFFFFE SPDDOWNACCEL EQU -2 ; slow down acceleration 29: =00000002 SPDUPACCEL EQU 2 ; speed up acceleration 30: * 31: ************************************************************ 32: * Object Following Debug Code 33: ************************************************************ 34: * 35: #ifdef __DEBUGFOLLOW_ 57: #endif 58: 59: * Reaction table for following (Center|Left|Right) 60: ************************************************************ 61: B7A5 B86E FOLLOWTBL DC.W FOBJAVOID ; 000 - Object avoidance 62: B7A7 B800 DC.W FHARDR ; 001 - Hard right 63: B7A9 B815 DC.W FHARDL ; 010 - Hard left 64: B7AB B860 DC.W FCONT ; 011 - ERROR - do what we did last 65: B7AD B82A DC.W FFOWARD ; 100 - Foward 66: B7AF B83C DC.W FSOFTR ; 101 - Soft right 67: B7B1 B84E DC.W FSOFTL ; 110 - Soft left 68: B7B3 B860 DC.W FCONT ; 111 - ERROR - do what we did last 69: 70: * 71: ******************************************************************************* 72: * SUBROUTINE - FOLLOW 73: * Description: Performs obstacle following behavior by reading the values 74: * from the IR sensors and moving the robot accordingly. 75: * Input : None. 76: * Output : None. 77: * Destroys : None. 78: * Calls : None. 79: ******************************************************************************* 80: * 81: B7B5 34 [02] FOLLOW PSHX ; save register X 82: B7B6 3B [02] PSHD ; save register D 83: 84: * JSR BUMPED ; check to see if we bumped something 85: * TBEQ A,FOLLOW1 ; no bump, continue following behavior 86: * JMP MAINOUT ; we bumped something, quit program 87: 88: B7B7 16 B7C8 [04] FOLLOW1 JSR FGETDATA ; get data from following sensors 89: 90: #ifdef __PRINTFOLLOW_ 107: #endif 108: 109: B7BA FOLLOW2 110: B7BA CE B7A5 [02] LDX #FOLLOWTBL ; load address of reaction table 111: B7BD B6 0927 [03] LDAA FTBLIDX ; get the reaction table index 112: B7C0 48 [01] LSLA ; convert to 16-bit index 113: B7C1 EE E4 [03] LDX A,X ; get address of handling routine 114: B7C3 05 00 [03] JMP 0,X ; jump to appropriate routine 115: 116: B7C5 3A [03] FOLLOWX PULD ; restore register D 117: B7C6 30 [03] PULX ; restore register X 118: B7C7 3D [05] RTS ; return to caller 119: * 120: ******************************************************************************* 121: * SUBROUTINE - FGETDATA 122: * Description: Reads the values from the following IR detectors. 123: * Input : None. 124: * Output : LFVALUE,RFVALUE. 125: * Destroys : LFVALUE,RFVALUE. 126: * Calls : None. 127: ******************************************************************************* 128: * 129: B7C8 36 [02] FGETDATA PSHA ; save register A 130: B7C9 180B 00 0927 [04] MOVB #$00,FTBLIDX ; clear reaction table index 131: 132: B7CE 86 04 [01] LDAA #CENTERFOLLOW ; get data from center sensor 133: B7D0 16 B26C [04] JSR ANALOG ; 134: B7D3 7A 0925 [03] STAA CFVALUE ; save center sensor value 135: B7D6 81 55 [01] CMPA #MINFOLLOW ; did center see speeding car? 136: B7D8 25 04 [03] BLO FGETDATA1 ; no, so continue 137: B7DA 1C 0927 04 [04] BSET FTBLIDX,BIT2 ; yes, so set bit in index 138: 139: B7DE 86 05 [01] FGETDATA1 LDAA #LEFTFOLLOW ; get data from left sensor 140: B7E0 16 B26C [04] JSR ANALOG ; 141: B7E3 7A 0924 [03] STAA LFVALUE ; save left sensor value 142: B7E6 81 55 [01] CMPA #MINFOLLOW ; did left see speeding car? 143: B7E8 25 04 [03] BLO FGETDATA2 ; no, so continue 144: B7EA 1C 0927 02 [04] BSET FTBLIDX,BIT1 ; yes, so set bit in index 145: 146: B7EE 86 06 [01] FGETDATA2 LDAA #RIGHTFOLLOW ; get data from right sensor 147: B7F0 16 B26C [04] JSR ANALOG ; 148: B7F3 7A 0926 [03] STAA RFVALUE ; save right sensor value 149: B7F6 81 55 [01] CMPA #MINFOLLOW ; did right see speeding car? 150: B7F8 25 04 [03] BLO FGETDATAX ; no, so get out 151: B7FA 1C 0927 01 [04] BSET FTBLIDX,BIT0 ; yes, so set bit in index 152: 153: B7FE 32 [03] FGETDATAX PULA ; restore register A 154: B7FF 3D [05] RTS ; return to caller 155: * 156: ******************************************************************************* 157: * SUBROUTINES - FHARDR,FHARDL,FFOWARD,FSOFTR,FSOFTL,FCONT 158: * Description: Handle motor control for following behavior 159: * Input : None. 160: * Output : None. 161: * Destroys : Reg X, Reg D. 162: * Calls : STEER. 163: ******************************************************************************* 164: * 165: B800 180C 0927 0928 [06] FHARDR MOVB FTBLIDX,LASTFOLLOW ; save off last table index 166: B806 CC 007D [02] LDD #FOLLOWTURNSPD ; set speed to turning speed 167: B809 16 B530 [04] JSR CHNGSPEED ; 168: B80C CE 0004 [02] LDX #HARDRIGHT ; turn hard right 169: B80F 16 B53D [04] JSR STEER ; 170: B812 06 B7C5 [03] JMP FOLLOWX ; get out 171: 172: B815 180C 0927 0928 [06] FHARDL MOVB FTBLIDX,LASTFOLLOW ; save off last table index 173: B81B CC 007D [02] LDD #FOLLOWTURNSPD ; set speed to turning speed 174: B81E 16 B530 [04] JSR CHNGSPEED ; 175: B821 CE 0002 [02] LDX #HARDLEFT ; turn hard left 176: B824 16 B53D [04] JSR STEER ; 177: B827 06 B7C5 [03] JMP FOLLOWX ; get out 178: 179: B82A 180C 0927 0928 [06] FFOWARD MOVB FTBLIDX,LASTFOLLOW ; save off last table index 180: B830 16 B896 [04] JSR SPEEDCALC ; go set new speed 181: B833 CE 0000 [02] LDX #GOFOWARD ; go foward 182: B836 16 B53D [04] JSR STEER ; 183: B839 06 B7C5 [03] JMP FOLLOWX ; get out 184: 185: B83C 180C 0927 0928 [06] FSOFTR MOVB FTBLIDX,LASTFOLLOW ; save off last table index 186: B842 16 B896 [04] JSR SPEEDCALC ; go set new speed 187: B845 CE 0005 [02] LDX #SOFTRIGHT ; turn soft right 188: B848 16 B53D [04] JSR STEER ; 189: B84B 06 B7C5 [03] JMP FOLLOWX ; get out 190: 191: B84E 180C 0927 0928 [06] FSOFTL MOVB FTBLIDX,LASTFOLLOW ; save off last table index 192: B854 16 B896 [04] JSR SPEEDCALC ; go set new speed 193: B857 CE 0003 [02] LDX #SOFTLEFT ; turn soft left 194: B85A 16 B53D [04] JSR STEER ; 195: B85D 06 B7C5 [03] JMP FOLLOWX ; get out 196: 197: B860 CE B7A5 [02] FCONT LDX #FOLLOWTBL ; load address of reaction table 198: B863 B6 0928 [03] LDAA LASTFOLLOW ; get the last reaction index 199: B866 7A 0927 [03] STAA FTBLIDX ; save as current reaction 200: B869 48 [01] LSLA ; convert to 16-bit index 201: B86A EE E4 [03] LDX A,X ; get address of handling routine 202: B86C 05 00 [03] JMP 0,X ; jump to appropriate routine 203: 204: * 205: ******************************************************************************* 206: * SUBROUTINE - FOBJAVOID 207: * Description: Handles object avoidance for following behavior 208: * Input : None. 209: * Output : None. 210: * Destroys : Reg X, Reg D. 211: * Calls : SIRENON,SIRENOFF,OBJAVOID,WAIT. 212: ******************************************************************************* 213: * 214: B86E 16 B37F [04] FOBJAVOID JSR SIRENOFF ; turn off siren to indicate we lost speeder 215: B871 CC 0064 [02] LDD #OBJAVOIDSPD ; set motor speed to obj avoid speed 216: B874 16 B530 [04] JSR CHNGSPEED ; 217: 218: B877 16 B642 [04] FOBJAVOID1 JSR OBJAVOID ; avoid obstacles 219: B87A CE 000A [02] LDX #OAPROCRATE ; wait designated amount of time 220: B87D 16 B171 [04] JSR WAIT ; 221: 222: B880 16 B7C8 [04] JSR FGETDATA ; check following sensors 223: B883 F7 0927 [04] TST FTBLIDX ; did we find the speeder? 224: B886 27 EF [03] BEQ FOBJAVOID1 ; no, continue object avoidance 225: 226: B888 16 B36A [04] JSR SIRENON ; turn siren on to indicate active chase 227: 228: B88B CE B7A5 [02] LDX #FOLLOWTBL ; load address of reaction table 229: B88E B6 0927 [03] LDAA FTBLIDX ; get the reaction table index 230: B891 48 [01] LSLA ; convert to 16-bit index 231: B892 EE E4 [03] LDX A,X ; get address of handling routine 232: B894 05 00 [03] JMP 0,X ; jump to appropriate routine 233: * 234: ******************************************************************************* 235: * SUBROUTINE - SPEEDCALC 236: * Description: Calculates and sets the next max motor speed so that the robot 237: * has a constant acceleration 238: * Input : None. 239: * Output : None. 240: * Destroys : Reg X, Reg D. 241: * Calls : . 242: ******************************************************************************* 243: * 244: B896 FC 092B [03] SPEEDCALC LDD NEWCFAVG ; load the current working average 245: B899 FB 0925 [03] ADDB CFVALUE ; add the newest center sensor 246: B89C 89 00 [01] ADCA #$00 ; to the current average 247: B89E FE 092D [03] LDX AVGCNT ; get the count of # items in working average 248: B8A1 08 [01] INX ; increment the count 249: B8A2 7E 092D [03] STX AVGCNT ; save the count 250: B8A5 8E 0014 [02] CPX #NUMAVGCNT ; do we have the correct sum yet? 251: B8A8 2D 47 [03] BLT SPEEDCALCX ; no, get out 252: 253: B8AA 1815 [0C] IDIVS ; calculate average 254: B8AC B7C5 [01] XGDX ; get average 255: B8AE 7C 092B [03] STD NEWCFAVG ; save average 256: 257: #ifdef __PRINTFOLLOW_ 268: #endif 269: 270: B8B1 B3 0929 [03] SUBD OLDCFAVG ; calc difference between old,new average 271: B8B4 8C 0001 [02] CPD #SPDACCEL ; compare to the desired acceleration 272: B8B7 27 26 [03] BEQ SPEEDCALC4 ; they are equal, do nothing 273: B8B9 2D 13 [03] BLT SPEEDCALC2 ; need to speed up 274: * SLOW DOWN 275: B8BB FC 0916 [03] LDD MAXFOWARDSPD ; get the current max speed 276: B8BE C3 FFFE [02] ADDD #SPDDOWNACCEL ; slow it down 277: B8C1 8C 0064 [02] CPD #MINFOLLOWSPD ; have we slowed down too far? 278: B8C4 2C 03 [03] BGE SPEEDCALC1 ; no, skip adjustment 279: B8C6 CC 0064 [02] LDD #MINFOLLOWSPD ; make speed = minimum speed 280: B8C9 16 B530 [04] SPEEDCALC1 JSR CHNGSPEED ; set the new speed 281: B8CC 20 11 [03] BRA SPEEDCALC4 ; get out 282: 283: * SPEED UP 284: B8CE FC 0916 [03] SPEEDCALC2 LDD MAXFOWARDSPD ; get the current max speed 285: B8D1 C3 0002 [02] ADDD #SPDUPACCEL ; speed it up 286: B8D4 8C 00C8 [02] CPD #MAXFOLLOWSPD ; have we speed up too far? 287: B8D7 2F 03 [03] BLE SPEEDCALC3 ; no, skip adjustment 288: B8D9 CC 00C8 [02] LDD #MAXFOLLOWSPD ; make speed = maximum speed 289: B8DC 16 B530 [04] SPEEDCALC3 JSR CHNGSPEED ; set the new speed 290: 291: B8DF 1804 092B 0929 [06] SPEEDCALC4 MOVW NEWCFAVG,OLDCFAVG ; make new average the old average 292: B8E5 1803 0000 092B [05] MOVW #$0000, NEWCFAVG ; clear new average to start over 293: B8EB 1803 0000 092D [05] MOVW #$0000,AVGCNT ; clear average count for next time 294: 295: B8F1 3D [05] SPEEDCALCX RTS ; return to caller 296: * 297: ******************************************************************************* Symbols: _1_4_speed 00000032 _1_8_speed 00000019 _3_4_speed 00000096 _3_8_speed 0000004b _5_8_speed *0000007d _7_8_speed 000000af __hc12_asm_ 00000001 __printbump_ 00000001 accelconst *00000006 adr0 00000070 adr0h 00000070 adr0l 00000071 adr1 00000072 adr1h 00000072 adr1l 00000073 adr2 00000074 adr2h *00000074 adr2l 00000075 adr3 00000076 adr3h 00000076 adr3l 00000077 adr4 00000078 adr4h 00000078 adr4l 00000079 adr5 0000007a adr5h 0000007a adr5l 0000007b adr6 0000007c adr6h 0000007c adr6l 0000007d adr7 0000007e adr7h 0000007e adr7l 0000007f analog *0000b26c analog1 *0000b26e atd_ivect 0000ffd2 atd_pvect 00000809 atdctl0 *00000060 atdctl1 00000061 atdctl2 *00000062 atdctl3 *00000063 atdctl4 *00000064 atdctl5 *00000065 atdstat 00000066 atdstath *00000066 atdstatl 00000067 atdtst 00000068 atdtsth 00000068 atdtstl 00000069 avgcnt *0000092d backleft *00000006 backright *00000007 backup *0000b729 backup1 0000b731 backupl *0000b740 backuptime *00000096 backupx *0000b746 bard 000000fc baud1200 0000000a baud14400 00000002 baud19200 00000000 baud2400 00000008 baud300 0000000e baud4800 00000006 baud600 0000000c baud9600 *00000004 baudtbl *0000b1b2 bcr1 000000f8 bcr2 000000fa bdlc_ivect 0000ffd0 bdlc_pvect 00000806 bdr 000000fb bit0 *00000001 bit1 *00000002 bit2 *00000004 bit3 *00000008 bit4 *00000010 bit5 *00000020 bit6 *00000040 bit7 *00000080 brkah 00000022 brkal 00000023 brkct0 00000020 brkct1 00000021 brkdh 00000024 brkdl 00000025 bspdflag *00000003 bsvr 000000f9 bumped *0000b1ae bumper *00000007 bumpstr *0000b161 bumpvalue *0000092e centerfollow *00000004 centerir *00000002 cforc *00000081 cfvalue *00000925 channel0 *00000000 channel1 *00000001 channel2 *00000002 channel3 00000003 channel4 *00000004 channel5 *00000005 channel6 *00000006 channel7 *00000007 chngspeed *0000b530 cntrtbl *0000b606 convreact *0000b6ce cop_clk_fail_ivect 0000fff0 cop_clk_fail_pvect 00000845 cop_fail_ivect 0000fff0 cop_fail_pvect 00000842 copctl *00000016 coprst 00000017 cr *0000000d creact *0000091f currman *00000922 cvalue *0000091e ddra 00000002 ddrb 00000003 ddrdlc 000000ff ddre 00000009 ddrp *00000057 ddrs 000000d7 ddrt 000000af dlcscr 000000fd dlyct 000000a9 eemcr 000000f0 eeprog 000000f3 eeprot 000000f1 eetst 000000f2 eos *00000004 esc 0000001b fcont *0000b860 feectl 000000f7 feelck 000000f4 feemcr 000000f5 feetst 000000f6 ffoward *0000b82a fgetdata *0000b7c8 fgetdata1 *0000b7de fgetdata2 *0000b7ee fgetdatax *0000b7fe fhardl *0000b815 fhardr *0000b800 flashoff 0000b364 flashon *0000b35e flashpat1 *0000000f flashpat2 000000f0 flashprev *00000909 flashrate *0000000f flashtimer *00000908 fobjavoid *0000b86e fobjavoid1 *0000b877 follow *0000b7b5 follow1 0000b7b7 follow2 0000b7ba followtbl *0000b7a5 followturnspd *0000007d followx *0000b7c5 fprocrate *0000000a frontrear *00000047 fsoftl *0000b84e fsoftr *0000b83c ftblidx *00000927 fullspeed *000000c8 getvalues *0000b6b3 glblvars *00000900 goback *00000001 gofoward *00000000 halfspeed *00000064 hard *0000b755 hard1 *0000b764 hardl *0000b774 hardleft *00000002 hardr 0000b76b hardright *00000004 hprio 0000001f icovw 000000aa icpacr 000000a8 icsys 000000ab inchar 0000b206 inchar1 *0000b20c incharwait *0000b1ff initatd *0000b249 initatd1 *0000b25b initee 00000012 initpwm *0000b487 initrg 00000011 initrm 00000010 initsci *0000b23b inittime *0000b275 intcr 0000001e inv0 000000fe inv1 000000fd inv2 000000fb inv3 000000f7 inv4 000000ef inv5 000000df inv6 000000bf inv7 0000007f io_base *00000000 irq_ivect 0000fff0 irq_pvect 00000836 killatd *0000b261 killpwm *0000b4d8 killtime *0000b328 lastfollow *00000928 led1port *00006000 leftbrk 00000080 leftdir *00000040 leftfollow *00000005 leftir *00000000 leftirstr 0000b085 leftmotor *0000b4de leftmotor1 *0000b4fd leftmotor2 *0000b501 leftmotorx 0000b506 leftright *0000b6fb leftright1 *0000b71b leftright2 *0000b722 leftrightr *0000b716 leftrightx *0000b727 leftspdtime *00000902 leftspstr 0000b0ab leftsts *0000b38e leftsts1 *0000b3ab leftsts2 *0000b3bd leftsts3 *0000b3c3 leftsts4 *0000b3cd leftstsx *0000b3d1 lefttbl *0000b61a lf *0000000a lfvalue *00000924 lreact *0000091b lrequal *00000010 lspdflag *00000001 lvalue *0000091a main *0000b0dd main2 *0000b10f mainout 0000b11a mainout1 *0000b128 mainoutx *0000b15f maxbackspd *00000918 maxfollowspd *000000c8 maxfowardspd *00000916 mccnt 000000b6 mccnth 000000b6 mccntl 000000b7 mcctl 000000a6 mcflg 000000a7 minfollow *00000055 minfollowspd *00000064 misc 00000013 mod_under_ivect 0000ffcc mod_under_pvect 00000800 mode 0000000b newcfavg *0000092b newline *0000b0a8 nibtochar *0000b220 nibtochar1 *0000b228 nibtochar2 *0000b22a nleftspd *00000910 noaction *00000000 nobumpmax *00000009 nrightspd *00000914 numavgcnt *00000014 oaprocrate *0000000a objavoid *0000b642 objavoid1 *0000b65e objavoid10 0000b6a2 objavoid2 *0000b666 objavoid3 *0000b66e objavoid4 *0000b678 objavoid5 *0000b680 objavoid6 *0000b688 objavoid7 *0000b692 objavoid8 *0000b69a objavoid9 *0000b6a2 objavoidb *0000b64e objavoidspd *00000064 objavoidx *0000b6b0 oc7d *00000083 oc7m *00000082 oldcfavg *00000929 oleftspd *0000090e orightspd *00000912 outaddr 0000b22e outchar *0000b1e9 outnum *0000b20d outstr *0000b1ef outstr1 *0000b1f1 outstr2 *0000b1fc pa0h 000000b5 pa1h 000000b4 pa2h 000000b3 pa3h 000000b2 pac_in_edge_ivect 0000ffda pac_in_edge_pvect 00000812 pac_over_ivect 0000ffdc pac_over_pvect 00000815 pacb_over_ivect 0000ffca pacb_over_pvect 00000803 pacn0 000000a5 pacn1 000000a4 pacn2 000000a3 pacn3 000000a2 pactl *000000a0 paflg 000000a1 pbctl 000000b0 pbflg 000000b1 pear 0000000a porta 00000000 portad 0000006f portb 00000001 portdlc 000000fe porte 00000008 portp *00000056 ports 000000d6 portt 000000ae prevrand *00000920 prevreact *00000921 progstart *0000b000 pucr 0000000c pullout *0000b5ce pulloutl *0000b5f7 pulloutr 0000b5e9 pulloutx *0000b603 purds 000000db pwclk *00000040 pwcnt0 00000048 pwcnt1 00000049 pwcnt2 0000004a pwcnt3 0000004b pwctl *00000054 pwdty0 *00000050 pwdty1 *00000051 pwdty2 *00000052 pwdty3 *00000053 pwen *00000042 pwm0 00000000 pwm1 00000001 pwm2 00000002 pwm3 00000003 pwm4 00000004 pwm5 00000005 pwm6 00000006 pwm7 00000007 pwper0 *0000004c pwper1 *0000004d pwper2 *0000004e pwper3 *0000004f pwpol *00000041 pwpres 00000043 pwscal0 *00000044 pwscal1 *00000046 pwscnt0 00000045 pwscnt1 00000047 pwtst 00000055 rdriv 0000000d reactback *00000001 reacthard *00000002 reactleft *00000001 reactrand 00000000 reactright *00000002 reactsoft *00000003 res4_ivect 0000ffd4 reset_ivect 0000fff0 reset_pvect 00000848 rfvalue *00000926 rightbrk 00000020 rightdir *00000010 rightfollow *00000006 rightir *00000001 rightirstr 0000b096 rightmotor *0000b507 rightmotor1 *0000b526 rightmotor2 *0000b52a rightmotorx 0000b52f rightspdtime *00000904 rightspstr 0000b0be rightsts *0000b3d2 rightsts1 *0000b3ef rightsts2 *0000b401 rightsts3 *0000b407 rightsts4 *0000b411 rightstsx *0000b415 righttbl *0000b62e rreact *0000091d rspdflag *00000002 rti_ivect 0000fff0 rti_pvect *00000833 rtictl *00000014 rtiflg *00000015 rvalue *0000091c rx_int_off 0000b1e5 rx_int_on 0000b1e1 rx_off 0000b1dd rx_on *0000b1d9 sc0bd *000000c0 sc0bdh 000000c0 sc0bdl 000000c1 sc0cr1 000000c2 sc0cr2 *000000c3 sc0drh 000000c6 sc0drl *000000c7 sc0sr1 *000000c4 sc0sr2 000000c5 sci0_ivect 0000ffd6 sci0_pvect 0000080c seg7port *00004000 set_baud *0000b1c7 shutdown *0000b053 siren *0000b476 sirenfreq *0000090a sirenfreq1 *000007d0 sirenfreq2 *000004b0 sirenoff *0000b37f sirenon *0000b36a sirenrate1 *fffffff6 sirenrate2 *0000000a sirentimer *0000090c sirenx *0000b486 slow 000000e0 soft *0000b77d soft1 *0000b78c softl *0000b79c softleft *00000003 softr 0000b793 softright *00000005 sp0br 000000d2 sp0cr1 000000d0 sp0cr2 000000d1 sp0dr 000000d5 sp0sr 000000d3 spdaccel *00000001 spdcaught *00000907 spddownaccel *fffffffe spdlimit *0000001f spdtimeflg *00000906 spdupaccel *00000002 speed 0000b0d2 speedcalc *0000b896 speedcalc1 *0000b8c9 speedcalc2 *0000b8ce speedcalc3 *0000b8dc speedcalc4 *0000b8df speedcalcx *0000b8f1 spi_tc_ivect 0000ffd8 spi_tc_pvect 0000080f stackptr *00000a00 steer *0000b53d steer1 *0000b550 steer2 *0000b55e steer3 *0000b56f steer4 *0000b57e steer5 *0000b58f steer6 *0000b59e steer7 *0000b5ad steer8 *0000b5bc steerx *0000b5cb stop *00000008 stopspeed 00000000 swi_ivect 0000fff0 swi_pvect 0000083c t0_ivect 0000ffee t0_pvect *00000830 t1_ivect 0000ffec t1_pvect *0000082d t2_ivect 0000ffea t2_pvect 0000082a t3_ivect 0000ffe8 t3_pvect 00000827 t4_ivect 0000ffe6 t4_pvect 00000824 t5_ivect 0000ffe4 t5_pvect 00000821 t6_ivect 0000ffe2 t6_pvect *0000081e t7_ivect 0000ffe0 t7_pvect 0000081b tc0 *00000090 tc0h 00000090 tc0l 00000091 tc1 *00000092 tc1h 00000092 tc1l 00000093 tc2 *00000094 tc2h 00000094 tc2l 00000095 tc3 *00000096 tc3h 00000096 tc3l 00000097 tc4 *00000098 tc4h 00000098 tc4l 00000099 tc5 *0000009a tc5h 0000009a tc5l 0000009b tc6 *0000009c tc6h 0000009c tc6l 0000009d tc7 *0000009e tc7h 0000009e tc7l 0000009f tcnt 00000084 tcnth 00000084 tcntl *00000085 tctl1 *00000088 tctl2 *00000089 tctl3 *0000008a tctl4 *0000008b tflg1 *0000008e tflg2 *0000008f timeext *0000b416 timeext1 *0000b43e timeext2 *0000b449 timeextx *0000b465 timtst 000000ad tios *00000080 tmr_over_ivect 0000ffde tmr_over_pvect *00000818 tmsk1 *0000008c tmsk2 *0000008d trap_ivect 0000fff0 trap_pvect 0000083f tscr *00000086 tx_off 0000b1d5 tx_on *0000b1d1 updmotors *0000b466 updmotorsx *0000b475 uppertimer *00000900 userprog_pvect *0000084b wait *0000b171 wait1 *0000b17b wait2 *0000b17d wait_tc *0000b1c2 waitbx *0000b198 waitspeed *0000b351 waitspeed1 *0000b351 waitx *0000b195 welcome *0000b000 xirq_ivect 0000fff0 xirq_pvect 00000839