                        .module mike_2.c
                        .area memory(abs)
                        .org 0x3e5e
 3E5E           _IRQ_TOF_vector::
 3E5E 03D5              .word _TOF_isr
                        .area data
                        .area memory(abs)
                        .org 0x3e52
 3E52           _IRQ_ATD0_vector::
 3E52 053B              .word _ATD0_Interrupt
                        .area data
                        .area memory(abs)
                        .org 0x3e50
 3E50           _IRQ_ATD1_vector::
 3E50 0588              .word _ATD1_Interrupt
                        .area data
                        .area memory(abs)
                        .org 0x3e64
 3E64           _IRQ_OC5_vector::
 3E64 0717              .word _OC5_ISR
                        .area data
                        .area memory(abs)
                        .org 0x3e6e
 3E6E           _IRQ_IC0_vector::
 3E6E 07C5              .word _IC0_ISR
                        .area data
                        .area memory(abs)
                        .org 0x3e6c
 3E6C           _IRQ_IC1_vector::
 3E6C 07F7              .word _IC1_ISR
                        .area data
                        .area text
 0000           _line_threshold::
 0000 005A              .word 90
 0002           _Intersection_threshold::
 0002 0050              .word 80
 0004           _Backup_threshold::
 0004 005A              .word 90
 0006           ;  lreg1 -> -4,x
 0006           ;  lreg2 -> -8,x
 0006           ;              b -> -12,x
 0006           ;              a -> -10,x
 0006           _main::
 0006 34                pshx
 0007 B775              tfr s,x
 0009 1B94              leas -12,sp
 000B           ; //*****************************************************************************
 000B           ; // Name:                              2007 Line Track code
 000B           ; // Vers:                          0.1
 000B           ; // Last Revised:              1/27/07 10:16pm
 000B           ; //*****************************************************************************                                       
 000B           ; 
 000B           ; #include<mc9S12dp256.h>
 000B           ; #include<stdio.h>
 000B           ; 
 000B           ; //define 16 bit PWM registers
 000B           ; #define PWMPERW1 _LP(0xB4) 
 000B           ; #define PWMPERW3 _LP(0xB6)
 000B           ; #define PWMPERW5 _LP(0xB8)     
 000B           ; #define PWMPERW7 _LP(0xBA)
 000B           ; 
 000B           ; #define PWMDTYW1 _LP(0xBC)
 000B           ; #define PWMDTYW3 _LP(0xBE)
 000B           ; #define PWMDTYW5 _LP(0xC0)
 000B           ; #define PWMDTYW7 _LP(0xC2)
 000B           ; 
 000B           ; // Function Prototypes
 000B           ; void init_pwm(void);
 000B           ; void init_timer(void);
 000B           ; void ob_led(int);
 000B           ; void pause(int);
 000B           ; void s_pause(int);
 000B           ; void init_tof(void);
 000B           ; void init_atd0(void);
 000B           ; void init_atd1(void);
 000B           ; void Speaker(void);
 000B           ; void stop(void);
 000B           ; void forward(void);
 000B           ; void reverse(void);
 000B           ; void left(void);
 000B           ; void right(void);
 000B           ; void motorspeed(void);
 000B           ; void speedcontrol(void);
 000B           ; void linetrack(int);
 000B           ; 
 000B           ; //General 
 000B           ; #define CLI asm("CLI")
 000B           ; #define SEI asm("SEI")
 000B           ; 
 000B           ; //TOF Interrupt Setup
 000B           ; void TOF_isr(void); 
 000B           ; #pragma interrupt_handler TOF_isr
 000B           ; #pragma abs_address: 0x3e5e
 000B           ; void (*IRQ_TOF_vector[])()={TOF_isr};
 000B           ; #pragma end_abs_address
 000B           ; 
 000B           ; //ATD0 Interrupt Setup
 000B           ; void ATD0_Interrupt();
 000B           ; #pragma interrupt_handler ATD0_Interrupt()
 000B           ; #pragma abs_address: 0x3E52
 000B           ; void (*IRQ_ATD0_vector[])()={ATD0_Interrupt};
 000B           ; #pragma end_abs_address
 000B           ; 
 000B           ; //ATD1 Interrupt Setup
 000B           ; void ATD1_Interrupt();
 000B           ; #pragma interrupt_handler ATD1_Interrupt()
 000B           ; #pragma abs_address: 0x3E50
 000B           ; void (*IRQ_ATD1_vector[])()={ATD1_Interrupt};
 000B           ; #pragma end_abs_address
 000B           ; 
 000B           ; //OC5 Interrupt Setup
 000B           ; void OC5_ISR();
 000B           ; #pragma interrupt_handler OC5_ISR()
 000B           ; #pragma abs_address: 0x3E64
 000B           ; void (*IRQ_OC5_vector[])()={OC5_ISR};
 000B           ; #pragma end_abs_address
 000B           ; 
 000B           ; //IC0 Interrupt Setup
 000B           ; void IC0_ISR();
 000B           ; #pragma interrupt_handler IC0_ISR()
 000B           ; #pragma abs_address: 0x3E6E
 000B           ; void (*IRQ_IC0_vector[])()={IC0_ISR};
 000B           ; #pragma end_abs_address
 000B           ; 
 000B           ; //IC1 Interrupt Setup
 000B           ; void IC1_ISR();
 000B           ; #pragma interrupt_handler IC1_ISR()
 000B           ; #pragma abs_address: 0x3E6C
 000B           ; void (*IRQ_IC1_vector[])()={IC1_ISR};
 000B           ; #pragma end_abs_address
 000B           ; 
 000B           ; //declare global variable
 000B           ; long int t_counts;
 000B           ; long int t_tenths;
 000B           ; long int t_hundredths;
 000B           ; unsigned int atd0[8];
 000B           ; unsigned int atd1[8];
 000B           ; int dela;
 000B           ; int line;
 000B           ; const line_threshold = 90;
 000B           ; unsigned long int IC0_count;
 000B           ; unsigned long int IC1_count;
 000B           ; 
 000B           ; int Intersection;
 000B           ; const Intersection_threshold = 80;     //original = 60 
 000B           ; int Backup;
 000B           ; const Backup_threshold = 90;
 000B           ; 
 000B           ; 
 000B           ; void main(void)
 000B           ; {
 000B           ; 
 000B           ; 
 000B           ;    int a,b; 
 000B           ;    init_pwm();
 000B 160305            jsr _init_pwm
 000E           ;    init_tof();
 000E 1603C1            jsr _init_tof
 0011           ;    init_atd0();
 0011 160527            jsr _init_atd0
 0014           ;    init_atd1();
 0014 160574            jsr _init_atd1
 0017           ;    init_ic0_1();
 0017 1607B2            jsr _init_ic0_1
 001A           ;  
 001A           ;    pause(1);
 001A CC0001            ldd #1
 001D 1604A1            jsr _pause
 0020           ;    
 0020           ;    ATD0CTL5 = 0x30;    //start atd's
 0020 C630              ldab #48
 0022 7B0085            stab 0x85
 0025           ;    ATD1CTL5 = 0x30;
 0025 C630              ldab #48
 0027 7B0125            stab 0x125
 002A           ;      
 002A           ;    pause(1);             //pause to allow atd to read first time;
 002A CC0001            ldd #1
 002D 1604A1            jsr _pause
 0030 2030              bra L5
 0032           L4:
 0032 CC12C0            ldd #4800
 0035 7C00BE            std 0xbe
 0038 CC000A            ldd #10
 003B 1604A1            jsr _pause
 003E CC1194            ldd #4500
 0041 7C00BE            std 0xbe
 0044 CC000A            ldd #10
 0047 1604A1            jsr _pause
 004A CC12C0            ldd #4800
 004D 7C00BE            std 0xbe
 0050 CC000A            ldd #10
 0053 1604A1            jsr _pause
 0056 CC1194            ldd #4500
 0059 7C00BE            std 0xbe
 005C CC000A            ldd #10
 005F 1604A1            jsr _pause
 0062           L5:
 0062           ;    
 0062           ;    while(1)
 0062 20CE              bra L4
 0064           X0:
 0064           ;    {
 0064           ;     
 0064           ;     PWMDTYW3 = 4800;
 0064           ; 	pause(10);
 0064           ;     PWMDTYW3 = 4500;
 0064           ; 	pause(10);
 0064           ;     PWMDTYW3 = 4800;
 0064           ; 	pause(10);
 0064           ;     PWMDTYW3 = 4500;
 0064           ; 	pause(10);
 0064           ; 	
 0064           ; 	
 0064           ; 	}
 0064           ;    
 0064           ;    ob_led(15);
 0064 CC000F            ldd #15
 0067 1602CA            jsr _ob_led
 006A           ;   // while(atd0[3] > 10);                //wait for switch 2 to be pressed
 006A           ;   
 006A           ;     pause(50);
 006A CC0032            ldd #50
 006D 1604A1            jsr _pause
 0070           ;   
 0070           ;     ob_led(14);
 0070 CC000E            ldd #14
 0073 1602CA            jsr _ob_led
 0076           ;     pause(50);
 0076 CC0032            ldd #50
 0079 1604A1            jsr _pause
 007C 1820021F          lbra L8
 0080           L7:
 0080           ; 	
 0080           ; 	/*forward();
 0080           ; 	
 0080           ; 	 IC1_count=0;
 0080           ; 	   while (IC1_count < 2700);	  
 0080           ; 	   
 0080           ; 	
 0080           ; 	 linetrack(0);  			   //linetrack to T
 0080           ; 	  // ob_led(11); 
 0080           ; 	 
 0080           ; 	  forward();
 0080           ; 	  
 0080           ; 	  IC1_count=0;
 0080           ; 	   while (IC1_count < 2000);	  
 0080           ; 	   stop();  			   //center robot on line
 0080           ; 	   pause(10); 
 0080           ; 	
 0080           ; 	left();	
 0080           ; 			   //turn till sensor on line
 0080           ; 	
 0080           ; 	while(line==0)
 0080           ; 	 {
 0080           ; 	    ob_led(line);
 0080           ; 		
 0080           ; 	 }
 0080           ; 	linetrack(0);
 0080           ; 	
 0080           ; 	forward();
 0080           ; 	IC1_count=0; 
 0080           ; 	while (IC1_count < 2175);  // 860 * number
 0080           ; 	
 0080           ; 	 stop();
 0080           ; 	 pause(20);
 0080           ;      
 0080           ; 	 			   //turn till sensor on line
 0080           ; 	
 0080           ; 	
 0080           ; 	 left();		   		//turn till sensor on line heading towards
 0080           ; 	 IC1_count=0;
 0080           ; 	 while (IC1_count < 2700);
 0080           ; 	 pause(5);            //ball request zone
 0080           ; 	 
 0080           ; 	/* while(line==0)        
 0080           ; 	 {
 0080           ; 	    ob_led(line);
 0080           ; 	 }  */
 0080           ; 	 /*stop();
 0080           ; 	 pause(5);
 0080           ; 	 
 0080           ; 	 linetrack(0);	 		   // get to ball request zone
 0080           ; 
 0080           ; 	 /*stop();
 0080           ; 	 pause(5);
 0080           ; 	 forward();
 0080           ; 	 
 0080           ; 	 
 0080           ; 	 pause(5);
 0080           ; 	// forward(); */
 0080           ; 	 
 0080           ; 	 
 0080           ; 	 /*
 0080           ; 	 forward();
 0080           ; 	 while(Backup != 7);
 0080           ; 	
 0080           ; 	 stop();
 0080           ; 	 pause(5);
 0080           ; 	 
 0080           ; 	 IC1_count = 0;
 0080           ; 	 
 0080           ; 	 forward();
 0080           ; 	 
 0080           ; 	 
 0080           ; 	 while (IC1_count < 920);
 0080           ; 	 //linetrack(1);
 0080           ; 	 
 0080           ; 	 stop();
 0080           ; 	 pause(5);
 0080           ; 	 */
 0080           ; 	 
 0080           ; 	 while(1)
 0080           ; 	 {
 0080           ; 	 reverse();
 0080 16036D            jsr _reverse
 0083           L10:
 0083           L11:
 0083           ; 	 while(Backup != 7);
 0083 FC0000            ldd _Backup
 0086 8C0007            cpd #7
 0089 26F8              bne L10
 008B           ;        
 008B           ;        stop();
 008B 160358            jsr _stop
 008E           ;        pause(5);
 008E CC0005            ldd #5
 0091 1604A1            jsr _pause
 0094           ;        
 0094           ;        left();                                
 0094 1603AC            jsr _left
 0097           ;        IC1_count=0;
 0097 CD0851            ldy #L13
 009A 1802401C          movw 0,y,-4,x
 009E 1802421E          movw 2,y,-2,x
 00A2 CD0004            ldy #_IC1_count
 00A5 18021C40          movw -4,x,0,y
 00A9 18021E42          movw -2,x,2,y
 00AD           L14:
 00AD           L15:
 00AD           ; 	 while (IC1_count < 17000);   	//180 turn in Request zone
 00AD CD0004            ldy #_IC1_count
 00B0 1802401C          movw 0,y,-4,x
 00B4 1802421E          movw 2,y,-2,x
 00B8 CD084D            ldy #L17
 00BB 18024018          movw 0,y,-8,x
 00BF 1802421A          movw 2,y,-6,x
 00C3 160000            jsr __lcmp
 00C6 25E5              blo L14
 00C8           ;        
 00C8           ;        stop();
 00C8 160358            jsr _stop
 00CB           ;        pause(10);
 00CB CC000A            ldd #10
 00CE 1604A1            jsr _pause
 00D1           ;        
 00D1           ;        linetrack(0);                                  //line track to intersection
 00D1 CC0000            ldd #0
 00D4 160726            jsr _linetrack
 00D7           ;        stop();
 00D7 160358            jsr _stop
 00DA           ;        pause(10);
 00DA CC000A            ldd #10
 00DD 1604A1            jsr _pause
 00E0           ;        
 00E0           ;        forward();                                             //move off of intersection
 00E0 160382            jsr _forward
 00E3           ;        IC1_count=0;
 00E3 CD0851            ldy #L13
 00E6 1802401C          movw 0,y,-4,x
 00EA 1802421E          movw 2,y,-2,x
 00EE CD0004            ldy #_IC1_count
 00F1 18021C40          movw -4,x,0,y
 00F5 18021E42          movw -2,x,2,y
 00F9           L18:
 00F9           L19:
 00F9           ; 	 while (IC1_count < 2700);	
 00F9 CD0004            ldy #_IC1_count
 00FC 1802401C          movw 0,y,-4,x
 0100 1802421E          movw 2,y,-2,x
 0104 CD0849            ldy #L21
 0107 18024018          movw 0,y,-8,x
 010B 1802421A          movw 2,y,-6,x
 010F 160000            jsr __lcmp
 0112 25E5              blo L18
 0114           ;        
 0114           ;        linetrack(0);            //linetrack to rebound zone, stop at front line track
 0114 CC0000            ldd #0
 0117 160726            jsr _linetrack
 011A           ;        stop();
 011A 160358            jsr _stop
 011D           ;        pause(10);
 011D CC000A            ldd #10
 0120 1604A1            jsr _pause
 0123           ;        
 0123           ;        forward();
 0123 160382            jsr _forward
 0126           ;        pause(5);
 0126 CC0005            ldd #5
 0129 1604A1            jsr _pause
 012C           ;        
 012C           ;        forward();                                             //back linetrack into rebound zone
 012C 160382            jsr _forward
 012F           L22:
 012F           L23:
 012F           ; 	 while(Backup != 7);
 012F FC0000            ldd _Backup
 0132 8C0007            cpd #7
 0135 26F8              bne L22
 0137           ;       
 0137           ;        stop();
 0137 160358            jsr _stop
 013A           ;        pause(5);
 013A CC0005            ldd #5
 013D 1604A1            jsr _pause
 0140           ;        
 0140           ;        IC1_count = 0;
 0140 CD0851            ldy #L13
 0143 1802401C          movw 0,y,-4,x
 0147 1802421E          movw 2,y,-2,x
 014B CD0004            ldy #_IC1_count
 014E 18021C40          movw -4,x,0,y
 0152 18021E42          movw -2,x,2,y
 0156           ;        
 0156           ;        forward();                                     //move one approx. one inch into rebound zone
 0156 160382            jsr _forward
 0159           L25:
 0159           L26:
 0159           ; 	 
 0159           ; 	 while (IC1_count < 920);
 0159 CD0004            ldy #_IC1_count
 015C 1802401C          movw 0,y,-4,x
 0160 1802421E          movw 2,y,-2,x
 0164 CD0845            ldy #L28
 0167 18024018          movw 0,y,-8,x
 016B 1802421A          movw 2,y,-6,x
 016F 160000            jsr __lcmp
 0172 25E5              blo L25
 0174           ;        
 0174           ;        stop();
 0174 160358            jsr _stop
 0177           ;        pause(10);
 0177 CC000A            ldd #10
 017A 1604A1            jsr _pause
 017D           ;        
 017D           ;        reverse();                                     //move one approx. one inch out of rebound zone
 017D 16036D            jsr _reverse
 0180           L29:
 0180           L30:
 0180           ; 	 
 0180           ; 	 while (IC1_count < 3000);
 0180 CD0004            ldy #_IC1_count
 0183 1802401C          movw 0,y,-4,x
 0187 1802421E          movw 2,y,-2,x
 018B CD0841            ldy #L32
 018E 18024018          movw 0,y,-8,x
 0192 1802421A          movw 2,y,-6,x
 0196 160000            jsr __lcmp
 0199 25E5              blo L29
 019B           ;        
 019B           ;        stop();
 019B 160358            jsr _stop
 019E           ;        pause(5);
 019E CC0005            ldd #5
 01A1 1604A1            jsr _pause
 01A4           ;        
 01A4           ;        left();                                
 01A4 1603AC            jsr _left
 01A7           ;        IC1_count=0;
 01A7 CD0851            ldy #L13
 01AA 1802401C          movw 0,y,-4,x
 01AE 1802421E          movw 2,y,-2,x
 01B2 CD0004            ldy #_IC1_count
 01B5 18021C40          movw -4,x,0,y
 01B9 18021E42          movw -2,x,2,y
 01BD           L33:
 01BD           L34:
 01BD           ; 	 while (IC1_count < 19600);   	//180 turn in Rebound zone
 01BD CD0004            ldy #_IC1_count
 01C0 1802401C          movw 0,y,-4,x
 01C4 1802421E          movw 2,y,-2,x
 01C8 CD083D            ldy #L36
 01CB 18024018          movw 0,y,-8,x
 01CF 1802421A          movw 2,y,-6,x
 01D3 160000            jsr __lcmp
 01D6 25E5              blo L33
 01D8           ;        
 01D8           ;        stop();
 01D8 160358            jsr _stop
 01DB           ;        pause(10);
 01DB CC000A            ldd #10
 01DE 1604A1            jsr _pause
 01E1           ;        
 01E1           ;        forward();
 01E1 160382            jsr _forward
 01E4           ;        pause(7);
 01E4 CC0007            ldd #7
 01E7 1604A1            jsr _pause
 01EA           ;        
 01EA           ;        linetrack(0);                                  //line track to intersection
 01EA CC0000            ldd #0
 01ED 160726            jsr _linetrack
 01F0           ;        stop();
 01F0 160358            jsr _stop
 01F3           ;        pause(10);
 01F3 CC000A            ldd #10
 01F6 1604A1            jsr _pause
 01F9           ;        
 01F9           ;        forward();                                             //move off of intersection
 01F9 160382            jsr _forward
 01FC           ;        IC1_count=0;
 01FC CD0851            ldy #L13
 01FF 1802401C          movw 0,y,-4,x
 0203 1802421E          movw 2,y,-2,x
 0207 CD0004            ldy #_IC1_count
 020A 18021C40          movw -4,x,0,y
 020E 18021E42          movw -2,x,2,y
 0212           L37:
 0212           L38:
 0212           ; 	 while (IC1_count < 2700);	
 0212 CD0004            ldy #_IC1_count
 0215 1802401C          movw 0,y,-4,x
 0219 1802421E          movw 2,y,-2,x
 021D CD0849            ldy #L21
 0220 18024018          movw 0,y,-8,x
 0224 1802421A          movw 2,y,-6,x
 0228 160000            jsr __lcmp
 022B 25E5              blo L37
 022D           ;        
 022D           ;        stop();
 022D 160358            jsr _stop
 0230           ;        pause(10);
 0230 CC000A            ldd #10
 0233 1604A1            jsr _pause
 0236           ;        
 0236           ;        linetrack(0);                     // go to ball request zone
 0236 CC0000            ldd #0
 0239 160726            jsr _linetrack
 023C           ; 
 023C           ;        stop();
 023C 160358            jsr _stop
 023F           ;        pause(5);
 023F CC0005            ldd #5
 0242 1604A1            jsr _pause
 0245           ;        
 0245           ;        forward();
 0245 160382            jsr _forward
 0248           ;        pause(5);
 0248 CC0005            ldd #5
 024B 1604A1            jsr _pause
 024E           ;       
 024E           ;        forward();
 024E 160382            jsr _forward
 0251           L40:
 0251           L41:
 0251           ; 	 while(Backup != 7);
 0251 FC0000            ldd _Backup
 0254 8C0007            cpd #7
 0257 26F8              bne L40
 0259           ;       
 0259           ;        stop();
 0259 160358            jsr _stop
 025C           ;        pause(5);
 025C CC0005            ldd #5
 025F 1604A1            jsr _pause
 0262           ;        
 0262           ;        IC1_count = 0;
 0262 CD0851            ldy #L13
 0265 1802401C          movw 0,y,-4,x
 0269 1802421E          movw 2,y,-2,x
 026D CD0004            ldy #_IC1_count
 0270 18021C40          movw -4,x,0,y
 0274 18021E42          movw -2,x,2,y
 0278           ;        
 0278           ;        forward();
 0278 160382            jsr _forward
 027B           L43:
 027B           L44:
 027B CD0004            ldy #_IC1_count
 027E 1802401C          movw 0,y,-4,x
 0282 1802421E          movw 2,y,-2,x
 0286 CD0845            ldy #L28
 0289 18024018          movw 0,y,-8,x
 028D 1802421A          movw 2,y,-6,x
 0291 160000            jsr __lcmp
 0294 25E5              blo L43
 0296 160358            jsr _stop
 0299 CC0005            ldd #5
 029C 1604A1            jsr _pause
 029F           L8:
 029F 1820FDDD          lbra L7
 02A3           X1:
 02A3           ; 	 
 02A3           ; 	 
 02A3           ; 	 while (IC1_count < 920);
 02A3           ; 	 stop();
 02A3           ; 	 pause(5);
 02A3           ; 	 
 02A3           ; 	 }  
 02A3           ; 	   
 02A3           ; 	/* reverse();				   // head back to 4 way intersection
 02A3           ; 	 IC1_count = 0;
 02A3           ; 	  
 02A3           ; 	 while (IC1_count < 2850);
 02A3           ; 	
 02A3           ; 	
 02A3           ; 	 linetrack(0);
 02A3           ; 	 
 02A3           ;  	 stop();	  			   // at 4 way
 02A3           ; 	 pause(10);  
 02A3           ; 	 
 02A3           ; 	 reverse();				   // head back to 4 way intersection
 02A3           ; 	 
 02A3           ; 	 IC1_count = 0;
 02A3           ; 	  
 02A3           ; 	 while (IC1_count < 1300);
 02A3           ; 
 02A3           ; 	  linetrack(1);
 02A3           ; 	 
 02A3           ; 	 stop();
 02A3           ; 	 pause(10);  */
 02A3           ; 	
 02A3           ; 	// forward();	   			   //leave start box 
 02A3           ; 	// pause(5); 
 02A3           ;  	
 02A3           ; 	 //linetrack(0);  			   //linetrack to T
 02A3           ; 	
 02A3           ; 	/* forward();	   			   
 02A3           ; 	 IC1_count=0;
 02A3           ; 	 while (IC1_count < 2177); 
 02A3           ; 	 ob_led(12);
 02A3           ; 	 stop();	  
 02A3           ; 	 pause(5);
 02A3           ; 	 ob_led(11);   */ 
 02A3           ; 	
 02A3           ; 	
 02A3           ; 	
 02A3           ;      Speaker();
 02A3 16067B            jsr _Speaker
 02A6           ;       // printf ("line = %d %d\n\r",atd1[7],atd1[6]);
 02A6           ; 
 02A6           ; 
 02A6           ; }
 02A6           L3:
 02A6 B757              tfr x,s
 02A8 30                pulx
 02A9                   .dbline 0 ; func end
 02A9 3D                rts
 02AA           L47:
 02AA 003F              .word 63
 02AC 0006              .word 6
 02AE 005B              .word 91
 02B0 004F              .word 79
 02B2 0066              .word 102
 02B4 006D              .word 109
 02B6 007D              .word 125
 02B8 0007              .word 7
 02BA 007F              .word 127
 02BC 006F              .word 111
 02BE 0077              .word 119
 02C0 007C              .word 124
 02C2 0039              .word 57
 02C4 005E              .word 94
 02C6 0079              .word 121
 02C8 0071              .word 113
 02CA           ;            out -> -32,x
 02CA           ;              t -> 2,x
 02CA           _ob_led::
 02CA 3B                pshd
 02CB 34                pshx
 02CC B775              tfr s,x
 02CE 1BF1DE            leas -34,sp
 02D1           ; 
 02D1           ; 
 02D1           ; // Display value passed to function on on board 7-seg LED of MiniDragon Board
 02D1           ; void ob_led(int t)               
 02D1           ; {
 02D1           ; 
 02D1           ;   int out[16] = {0x3f,0x06,0x5b,0x4f,0x66,0x6d,0x7d,0x07,0x7f,0x6f,0x77,0x7c,
 02D1 19E1E0            leay -32,x
 02D4 B7C6              xgdy
 02D6 CD02AA            ldy #L47
 02D9 34                pshx
 02DA B745              tfr d,x
 02DC CC0010            ldd #16
 02DF           X2:
 02DF 18027131          movw 2,y+,2,x+
 02E3 0434F9            dbne d,X2
 02E6 30                pulx
 02E7           ;     0x39,0x5e,0x79,0x71};
 02E7           ;   
 02E7           ;   DDRH=0xff;
 02E7 C6FF              ldab #255
 02E9 7B0262            stab 0x262
 02EC           ;   
 02EC           ;   PTH=out[t];
 02EC 19E1E0            leay -32,x
 02EF 6DE1DE            sty -34,x
 02F2 EC02              ldd 2,x
 02F4 59                lsld
 02F5 E3E1DE            addd -34,x
 02F8 B7C6              xgdy
 02FA EC40              ldd 0,y
 02FC 7B0260            stab 0x260
 02FF           ; 
 02FF           ;   
 02FF           ; }
 02FF           L46:
 02FF B757              tfr x,s
 0301 30                pulx
 0302 1B82              leas 2,sp
 0304                   .dbline 0 ; func end
 0304 3D                rts
 0305           _init_pwm::
 0305           ; 
 0305           ; // Set up 4-16bit PWM 
 0305           ; void init_pwm()
 0305           ; {
 0305           ;   PWMTST = 0x00; 		   			  // turn off all PWM special Modes
 0305 7900A6            clr 0xa6
 0308           ;   PWMCAE = 0x00;                                        // PWM left aligned output mode 
 0308 7900A4            clr 0xa4
 030B           ;   PWMCTL = 0xf0;                                        // Use Concatenated Mode (16-bit)
 030B C6F0              ldab #240
 030D 7B00A5            stab 0xa5
 0310           ;   
 0310           ;   
 0310           ;   PWMPRCLK = 0x33;                                      // E/8 for A and B       
 0310 C633              ldab #51
 0312 7B00A3            stab 0xa3
 0315           ;   
 0315           ;   PWMCLK = 0x00;                                        // PW Chan 1, 5 clock A
 0315 7900A2            clr 0xa2
 0318           ;                                                                         // PW chan 3, 7 clock B
 0318           ;                                                                         
 0318           ;   PWMPOL= 0xaa;                                                 // High Polarity for all channels
 0318 C6AA              ldab #170
 031A 7B00A1            stab 0xa1
 031D           ;   DDRP= 0xaa;                                           // Set Port P to output 
 031D C6AA              ldab #170
 031F 7B025A            stab 0x25a
 0322           ; 
 0322           ;   PWMPERW1 = 60000;                                     //stop (7.5%) all motors
 0322 CCEA60            ldd #0xea60
 0325 7C00B4            std 0xb4
 0328           ;   PWMDTYW1 = 4500;                                      //<- in theory this value should be 4500
 0328 CC1194            ldd #4500
 032B 7C00BC            std 0xbc
 032E           ;   
 032E           ;   PWMPERW3 = 60000;                                   
 032E CCEA60            ldd #0xea60
 0331 7C00B6            std 0xb6
 0334           ;   PWMDTYW3 = 4500;                                      //<- in theory this value should be 4500
 0334 CC1194            ldd #4500
 0337 7C00BE            std 0xbe
 033A           ;   
 033A           ;   PWMPERW5 = 60000;                                   //tyw5 pin 111
 033A CCEA60            ldd #0xea60
 033D 7C00B8            std 0xb8
 0340           ;   PWMDTYW5 = 4500;                            // //tyw5 pin 111
 0340 CC1194            ldd #4500
 0343 7C00C0            std 0xc0
 0346           ; 
 0346           ;   PWMPERW7 = 60000;                           //tyw7 pin 109           
 0346 CCEA60            ldd #0xea60
 0349 7C00BA            std 0xba
 034C           ;   PWMDTYW7 = 4500;                                    //tyw7 pin 109
 034C CC1194            ldd #4500
 034F 7C00C2            std 0xc2
 0352           ;   PWME= 0xaa;                                           // Enable ALL PWM Channels
 0352 C6AA              ldab #170
 0354 7B00A0            stab 0xa0
 0357           ; 
 0357           ; }
 0357           L48:
 0357                   .dbline 0 ; func end
 0357 3D                rts
 0358           _stop::
 0358           ; 
 0358           ; // Stop
 0358           ; void stop()
 0358           ; {
 0358           ;    PWME = 0x00;
 0358 7900A0            clr 0xa0
 035B           ;    PWMDTYW5 = 4500;                       //4400 tyw5 pin 111
 035B CC1194            ldd #4500
 035E 7C00C0            std 0xc0
 0361           ;    PWMDTYW7 = 4500;                           //4600  tyw7 pin 109
 0361 CC1194            ldd #4500
 0364 7C00C2            std 0xc2
 0367           ;    PWME = 0xaa;
 0367 C6AA              ldab #170
 0369 7B00A0            stab 0xa0
 036C           ; }
 036C           L49:
 036C                   .dbline 0 ; func end
 036C 3D                rts
 036D           _reverse::
 036D           ;    
 036D           ; // reverse
 036D           ; void reverse()
 036D           ; {
 036D           ;    PWME = 0x00;
 036D 7900A0            clr 0xa0
 0370           ;    PWMDTYW5 = 4100;           //half full speed forward 4876//tyw5 pin 111 4100
 0370 CC1004            ldd #4100
 0373 7C00C0            std 0xc0
 0376           ;    PWMDTYW7 = 4900;           //half full speed reverse 4125// tyw7 pin 109 4900
 0376 CC1324            ldd #4900
 0379 7C00C2            std 0xc2
 037C           ;    PWME = 0xaa;
 037C C6AA              ldab #170
 037E 7B00A0            stab 0xa0
 0381           ; }
 0381           L50:
 0381                   .dbline 0 ; func end
 0381 3D                rts
 0382           _forward::
 0382           ; // forward
 0382           ; void forward()
 0382           ; {
 0382           ;    PWME = 0x00;
 0382 7900A0            clr 0xa0
 0385           ;    PWMDTYW5 = 4900;                           //half full speed reverse//tyw5 pin 111 4900
 0385 CC1324            ldd #4900
 0388 7C00C0            std 0xc0
 038B           ;    PWMDTYW7 = 4100;                           //half full speed forward//tyw7 pin 109 4100
 038B CC1004            ldd #4100
 038E 7C00C2            std 0xc2
 0391           ;    PWME = 0xaa;
 0391 C6AA              ldab #170
 0393 7B00A0            stab 0xa0
 0396           ; }
 0396           L51:
 0396                   .dbline 0 ; func end
 0396 3D                rts
 0397           _right::
 0397           ;    
 0397           ; // right
 0397           ; void right()
 0397           ; {
 0397           ;    PWME = 0x00;
 0397 7900A0            clr 0xa0
 039A           ;    PWMDTYW5 = 5000;                           //orignal speed 5250//tyw5 pin 111
 039A CC1388            ldd #5000
 039D 7C00C0            std 0xc0
 03A0           ;    PWMDTYW7 = 5000;                           //orignal speed 5250//tyw7 pin 109
 03A0 CC1388            ldd #5000
 03A3 7C00C2            std 0xc2
 03A6           ;    PWME = 0xaa;
 03A6 C6AA              ldab #170
 03A8 7B00A0            stab 0xa0
 03AB           ; }
 03AB           L52:
 03AB                   .dbline 0 ; func end
 03AB 3D                rts
 03AC           _left::
 03AC           ; 
 03AC           ; // left
 03AC           ; void left()
 03AC           ; {
 03AC           ;    PWME = 0x00;
 03AC 7900A0            clr 0xa0
 03AF           ;    PWMDTYW5 = 4000;                           //orignal speed 3750//tyw5 pin 111
 03AF CC0FA0            ldd #4000
 03B2 7C00C0            std 0xc0
 03B5           ;    PWMDTYW7 = 4000;                           //orignal speed 3750//tyw7 pin 109
 03B5 CC0FA0            ldd #4000
 03B8 7C00C2            std 0xc2
 03BB           ;    PWME = 0xaa;
 03BB C6AA              ldab #170
 03BD 7B00A0            stab 0xa0
 03C0           ; }
 03C0           L53:
 03C0                   .dbline 0 ; func end
 03C0 3D                rts
 03C1           _init_tof::
 03C1           ; 
 03C1           ; //Initialize Timer Overflow 
 03C1           ; void init_tof()
 03C1           ; {
 03C1           ;    SEI;
 03C1 1410                      SEI
 03C3           
 03C3           ;    TSCR1 = 0x80;                                        //turn on Timer
 03C3 C680              ldab #128
 03C5 7B0046            stab 0x46
 03C8           ;    TFLG2 = 0x80;                                        //clear TOF flag
 03C8 C680              ldab #128
 03CA 7B004F            stab 0x4f
 03CD           ;    TSCR2 = 0x80;                                        //turn on TOF interrupt
 03CD C680              ldab #128
 03CF 7B004D            stab 0x4d
 03D2           ;    CLI;
 03D2 10EF                      CLI
 03D4           
 03D4           ; }
 03D4           L54:
 03D4                   .dbline 0 ; func end
 03D4 3D                rts
 03D5           ;  lreg1 -> -4,x
 03D5           ;  lreg2 -> -8,x
 03D5           _TOF_isr::
 03D5 34                pshx
 03D6 B775              tfr s,x
 03D8 1B98              leas -8,sp
 03DA           ; 
 03DA           ; //Timer Overflow Counter (ISR)
 03DA           ; void TOF_isr()  
 03DA           ; {
 03DA           ;    t_counts++;
 03DA CD0038            ldy #_t_counts
 03DD 1802401C          movw 0,y,-4,x
 03E1 1802421E          movw 2,y,-2,x
 03E5 CD0839            ldy #L56
 03E8 18024018          movw 0,y,-8,x
 03EC 1802421A          movw 2,y,-6,x
 03F0 160000            jsr __ladd
 03F3 CD0038            ldy #_t_counts
 03F6 18021C40          movw -4,x,0,y
 03FA 18021E42          movw -2,x,2,y
 03FE           ;    if ((t_counts%38)==0)
 03FE CD0038            ldy #_t_counts
 0401 1802401C          movw 0,y,-4,x
 0405 1802421E          movw 2,y,-2,x
 0409 CD0835            ldy #L59
 040C 18024018          movw 0,y,-8,x
 0410 1802421A          movw 2,y,-6,x
 0414 160000            jsr __lmod
 0417 CD0831            ldy #L60
 041A 18024018          movw 0,y,-8,x
 041E 1802421A          movw 2,y,-6,x
 0422 160000            jsr __lcmp
 0425 2624              bne L57
 0427           ;    {
 0427           ;         t_tenths++;
 0427 CD0034            ldy #_t_tenths
 042A 1802401C          movw 0,y,-4,x
 042E 1802421E          movw 2,y,-2,x
 0432 CD0839            ldy #L56
 0435 18024018          movw 0,y,-8,x
 0439 1802421A          movw 2,y,-6,x
 043D 160000            jsr __ladd
 0440 CD0034            ldy #_t_tenths
 0443 18021C40          movw -4,x,0,y
 0447 18021E42          movw -2,x,2,y
 044B           ;    }
 044B           L57:
 044B           ;    if ((t_counts%4)==0)
 044B CD0038            ldy #_t_counts
 044E 1802401C          movw 0,y,-4,x
 0452 1802421E          movw 2,y,-2,x
 0456 CD082D            ldy #L63
 0459 18024018          movw 0,y,-8,x
 045D 1802421A          movw 2,y,-6,x
 0461 160000            jsr __lmod
 0464 CD0831            ldy #L60
 0467 18024018          movw 0,y,-8,x
 046B 1802421A          movw 2,y,-6,x
 046F 160000            jsr __lcmp
 0472 2624              bne L61
 0474           ;    {
 0474           ;       t_hundredths++;
 0474 CD0030            ldy #_t_hundredths
 0477 1802401C          movw 0,y,-4,x
 047B 1802421E          movw 2,y,-2,x
 047F CD0839            ldy #L56
 0482 18024018          movw 0,y,-8,x
 0486 1802421A          movw 2,y,-6,x
 048A 160000            jsr __ladd
 048D CD0030            ldy #_t_hundredths
 0490 18021C40          movw -4,x,0,y
 0494 18021E42          movw -2,x,2,y
 0498           ;    }
 0498           L61:
 0498           ;    
 0498           ;    TFLG2 = 0x80;
 0498 C680              ldab #128
 049A 7B004F            stab 0x4f
 049D           ; }
 049D           L55:
 049D B757              tfr x,s
 049F 30                pulx
 04A0                   .dbline 0 ; func end
 04A0 0B                rti
 04A1           ;  lreg1 -> -4,x
 04A1           ;  lreg2 -> -8,x
 04A1           ;       end_time -> -12,x
 04A1           ;     pause_time -> 2,x
 04A1           _pause::
 04A1 3B                pshd
 04A2 34                pshx
 04A3 B775              tfr s,x
 04A5 1B94              leas -12,sp
 04A7           ; 
 04A7           ; //pause for pause_time*0.1 sec
 04A7           ; void pause(int pause_time)
 04A7           ; {
 04A7           ;    long int end_time;
 04A7           ;    end_time = t_tenths + pause_time;
 04A7 CD0034            ldy #_t_tenths
 04AA 1802401C          movw 0,y,-4,x
 04AE 1802421E          movw 2,y,-2,x
 04B2 EC02              ldd 2,x
 04B4 160000            jsr __d2lreg2
 04B7 160000            jsr __ladd
 04BA 1914              leay -12,x
 04BC 18021C40          movw -4,x,0,y
 04C0 18021E42          movw -2,x,2,y
 04C4           L65:
 04C4           L66:
 04C4           ; //   printf ("%ld = %ld + %d\n\r",end_time,t_tenths,pause_time);
 04C4           ;    while(end_time>t_tenths)
 04C4 1914              leay -12,x
 04C6 1802401C          movw 0,y,-4,x
 04CA 1802421E          movw 2,y,-2,x
 04CE CD0034            ldy #_t_tenths
 04D1 18024018          movw 0,y,-8,x
 04D5 1802421A          movw 2,y,-6,x
 04D9 160000            jsr __lcmp
 04DC 2EE6              bgt L65
 04DE           ;    {
 04DE           ;    }
 04DE           ; }
 04DE           L64:
 04DE B757              tfr x,s
 04E0 30                pulx
 04E1 1B82              leas 2,sp
 04E3                   .dbline 0 ; func end
 04E3 3D                rts
 04E4           ;  lreg1 -> -4,x
 04E4           ;  lreg2 -> -8,x
 04E4           ;       end_time -> -12,x
 04E4           ;     pause_time -> 2,x
 04E4           _s_pause::
 04E4 3B                pshd
 04E5 34                pshx
 04E6 B775              tfr s,x
 04E8 1B94              leas -12,sp
 04EA           ; 
 04EA           ; //pause for pause_time*0.01 sec
 04EA           ; void s_pause(int pause_time)
 04EA           ; {
 04EA           ;    long int end_time;
 04EA           ;    end_time = t_hundredths + pause_time;
 04EA CD0030            ldy #_t_hundredths
 04ED 1802401C          movw 0,y,-4,x
 04F1 1802421E          movw 2,y,-2,x
 04F5 EC02              ldd 2,x
 04F7 160000            jsr __d2lreg2
 04FA 160000            jsr __ladd
 04FD 1914              leay -12,x
 04FF 18021C40          movw -4,x,0,y
 0503 18021E42          movw -2,x,2,y
 0507           L69:
 0507           L70:
 0507           ; //   printf ("%ld = %ld + %d\n\r",end_time,t_tenths,pause_time);
 0507           ;    while(end_time>t_hundredths)
 0507 1914              leay -12,x
 0509 1802401C          movw 0,y,-4,x
 050D 1802421E          movw 2,y,-2,x
 0511 CD0030            ldy #_t_hundredths
 0514 18024018          movw 0,y,-8,x
 0518 1802421A          movw 2,y,-6,x
 051C 160000            jsr __lcmp
 051F 2EE6              bgt L69
 0521           ;    {
 0521           ;    }
 0521           ; }
 0521           L68:
 0521 B757              tfr x,s
 0523 30                pulx
 0524 1B82              leas 2,sp
 0526                   .dbline 0 ; func end
 0526 3D                rts
 0527           _init_atd0::
 0527           ; 
 0527           ; // ******************* Initialize ATD *******************
 0527           ; void init_atd0()
 0527           ; {
 0527           ;    SEI;
 0527 1410                      SEI
 0529           
 0529           ;    ATD0CTL2 = 0xC2;
 0529 C6C2              ldab #194
 052B 7B0082            stab 0x82
 052E           ;    ATD0CTL3 = 0x40;
 052E C640              ldab #64
 0530 7B0083            stab 0x83
 0533           ;    ATD0CTL4 = 0xE7;
 0533 C6E7              ldab #231
 0535 7B0084            stab 0x84
 0538           ;    CLI;
 0538 10EF                      CLI
 053A           
 053A           ; }
 053A           L72:
 053A                   .dbline 0 ; func end
 053A 3D                rts
 053B           _ATD0_Interrupt::
 053B           ; 
 053B           ; /********************************************************************/
 053B           ; /* This code is primarily taken from example 7.3.5 in the text      */
 053B           ; /* and then converted to act as an ISR                              */
 053B           ; /********************************************************************/
 053B           ;   	  
 053B           ; void ATD0_Interrupt()
 053B           ; {
 053B           ; /*print results to PC screen for testing             */
 053B           ;    atd0[0]= ATD0DR0H;	
 053B           	; vol
 053B F60090            ldab 0x90
 053E 87                clra
 053F 7C0020            std _atd0
 0542           ;    atd0[1]= ATD0DR1H;
 0542                   ; vol
 0542 F60092            ldab 0x92
 0545 87                clra
 0546 7C0022            std _atd0+2
 0549           ;    atd0[2]= ATD0DR2H;
 0549                   ; vol
 0549 F60094            ldab 0x94
 054C 87                clra
 054D 7C0024            std _atd0+4
 0550           ;    atd0[3]= ATD0DR3H;
 0550                   ; vol
 0550 F60096            ldab 0x96
 0553 87                clra
 0554 7C0026            std _atd0+6
 0557           ;    atd0[4]= ATD0DR4H;
 0557                   ; vol
 0557 F60098            ldab 0x98
 055A 87                clra
 055B 7C0028            std _atd0+8
 055E           ;    atd0[5]= ATD0DR5H;
 055E                   ; vol
 055E F6009A            ldab 0x9a
 0561 87                clra
 0562 7C002A            std _atd0+10
 0565           ;    atd0[6]= ATD0DR6H;
 0565                   ; vol
 0565 F6009C            ldab 0x9c
 0568 87                clra
 0569 7C002C            std _atd0+12
 056C           ;    atd0[7]= ATD0DR7H;         
 056C                   ; vol
 056C F6009E            ldab 0x9e
 056F 87                clra
 0570 7C002E            std _atd0+14
 0573           ; 
 0573           ; }
 0573           L73:
 0573                   .dbline 0 ; func end
 0573 0B                rti
 0574           _init_atd1::
 0574           ; 
 0574           ; // ******************* Initialize ATD *******************
 0574           ; void init_atd1()
 0574           ; {
 0574           ;    SEI;
 0574 1410                      SEI
 0576           
 0576           ;    ATD1CTL2 = 0xC2;
 0576 C6C2              ldab #194
 0578 7B0122            stab 0x122
 057B           ;    ATD1CTL3 = 0x40;
 057B C640              ldab #64
 057D 7B0123            stab 0x123
 0580           ;    ATD1CTL4 = 0xE7;
 0580 C6E7              ldab #231
 0582 7B0124            stab 0x124
 0585           ;    CLI;
 0585 10EF                      CLI
 0587           
 0587           ; }
 0587           L81:
 0587                   .dbline 0 ; func end
 0587 3D                rts
 0588           _ATD1_Interrupt::
 0588           ; 
 0588           ; /********************************************************************/
 0588           ; /* This code is primarily taken from example 7.3.5 in the text      */
 0588           ; /* and then converted to act as an ISR                              */
 0588           ; /********************************************************************/
 0588           ;   	  
 0588           ; void ATD1_Interrupt()
 0588           ; {
 0588           ; /*print results to PC screen for testing             */
 0588           ;    atd1[0]= ATD1DR0H;	
 0588           	; vol
 0588 F60130            ldab 0x130
 058B 87                clra
 058C 7C0010            std _atd1
 058F           ;    atd1[1]= ATD1DR1H;
 058F                   ; vol
 058F F60132            ldab 0x132
 0592 87                clra
 0593 7C0012            std _atd1+2
 0596           ;    atd1[2]= ATD1DR2H;
 0596                   ; vol
 0596 F60134            ldab 0x134
 0599 87                clra
 059A 7C0014            std _atd1+4
 059D           ;    atd1[3]= ATD1DR3H;
 059D                   ; vol
 059D F60136            ldab 0x136
 05A0 87                clra
 05A1 7C0016            std _atd1+6
 05A4           ;    atd1[4]= ATD1DR4H;
 05A4                   ; vol
 05A4 F60138            ldab 0x138
 05A7 87                clra
 05A8 7C0018            std _atd1+8
 05AB           ;    atd1[5]= ATD1DR5H;
 05AB                   ; vol
 05AB F6013A            ldab 0x13a
 05AE 87                clra
 05AF 7C001A            std _atd1+10
 05B2           ;    atd1[6]= ATD1DR6H;
 05B2                   ; vol
 05B2 F6013C            ldab 0x13c
 05B5 87                clra
 05B6 7C001C            std _atd1+12
 05B9           ;    atd1[7]= ATD1DR7H;         
 05B9                   ; vol
 05B9 F6013E            ldab 0x13e
 05BC 87                clra
 05BD 7C001E            std _atd1+14
 05C0           ;  
 05C0           ; //Checking Line Track
 05C0           ;   line = 0;
 05C0 CC0000            ldd #0
 05C3 7C000C            std _line
 05C6           ;   
 05C6           ;   if (atd1[0] < line_threshold)
 05C6 FC0010            ldd _atd1
 05C9 BC0000            cpd _line_threshold
 05CC 2409              bhs L90
 05CE           ;   {
 05CE           ;     line += 0x1;
 05CE FC000C            ldd _line
 05D1 C30001            addd #1
 05D4 7C000C            std _line
 05D7           ;   }
 05D7           L90:
 05D7           ;   if (atd1[1] < line_threshold)
 05D7 FC0012            ldd _atd1+2
 05DA BC0000            cpd _line_threshold
 05DD 2409              bhs L92
 05DF           ;   {
 05DF           ;     line += 0x2;
 05DF FC000C            ldd _line
 05E2 C30002            addd #2
 05E5 7C000C            std _line
 05E8           ;    }
 05E8           L92:
 05E8           ;   if (atd1[2] < line_threshold)
 05E8 FC0014            ldd _atd1+4
 05EB BC0000            cpd _line_threshold
 05EE 2409              bhs L95
 05F0           ;    {
 05F0           ;     line += 0x4;
 05F0 FC000C            ldd _line
 05F3 C30004            addd #4
 05F6 7C000C            std _line
 05F9           ;    }
 05F9           L95:
 05F9           ; //Checking Intersection 
 05F9           ;   Intersection = 0;
 05F9 CC0000            ldd #0
 05FC 7C0002            std _Intersection
 05FF           ;   
 05FF           ;   if (atd1[6] < Intersection_threshold)
 05FF FC001C            ldd _atd1+12
 0602 BC0002            cpd _Intersection_threshold
 0605 2409              bhs L98
 0607           ;   {
 0607           ;     Intersection += 0x1;
 0607 FC0002            ldd _Intersection
 060A C30001            addd #1
 060D 7C0002            std _Intersection
 0610           ;   }
 0610           L98:
 0610           ;   if (atd1[7] < Intersection_threshold)
 0610 FC001E            ldd _atd1+14
 0613 BC0002            cpd _Intersection_threshold
 0616 2409              bhs L101
 0618           ;   {
 0618           ;     Intersection += 0x2;
 0618 FC0002            ldd _Intersection
 061B C30002            addd #2
 061E 7C0002            std _Intersection
 0621           ;    }
 0621           L101:
 0621           ; //Checking Backup 
 0621           ;   Backup = 0;
 0621 CC0000            ldd #0
 0624 7C0000            std _Backup
 0627           ;   
 0627           ;   if (atd1[3] < Backup_threshold)
 0627 FC0016            ldd _atd1+6
 062A BC0004            cpd _Backup_threshold
 062D 2409              bhs L104
 062F           ;   {
 062F           ;     Backup += 0x1;
 062F FC0000            ldd _Backup
 0632 C30001            addd #1
 0635 7C0000            std _Backup
 0638           ;   }
 0638           L104:
 0638           ;   if (atd1[4] < Backup_threshold)
 0638 FC0018            ldd _atd1+8
 063B BC0004            cpd _Backup_threshold
 063E 2409              bhs L107
 0640           ;   {
 0640           ;     Backup += 0x2;
 0640 FC0000            ldd _Backup
 0643 C30002            addd #2
 0646 7C0000            std _Backup
 0649           ;    }
 0649           L107:
 0649           ;   if (atd1[5] < Backup_threshold)
 0649 FC001A            ldd _atd1+10
 064C BC0004            cpd _Backup_threshold
 064F 2409              bhs L110
 0651           ;    {
 0651           ;     Backup += 0x4;
 0651 FC0000            ldd _Backup
 0654 C30004            addd #4
 0657 7C0000            std _Backup
 065A           ;    }
 065A           L110:
 065A           ;    
 065A           ;   // printf("%x %x %x %x\n\r", atd1[0], atd1[1], atd1[2], line); 	
 065A           ; }
 065A           L82:
 065A                   .dbline 0 ; func end
 065A 0B                rti
 065B           L114:
 065B BAB9              .word 47801
 065D A65D              .word 42589
 065F 9430              .word 37936
 0661 8BE9              .word 35817
 0663 7C90              .word 31888
 0665 6EF9              .word 28409
 0667 62D7              .word 25303
 0669 5D5C              .word 23900
 066B           L115:
 066B 0005              .word 5
 066D 0005              .word 5
 066F 0005              .word 5
 0671 0005              .word 5
 0673 0005              .word 5
 0675 0005              .word 5
 0677 0005              .word 5
 0679 0005              .word 5
 067B           ;              i -> -36,x
 067B           ;            dur -> -34,x
 067B           ;          score -> -18,x
 067B           ;              j -> -2,x
 067B           _Speaker::
 067B 34                pshx
 067C B775              tfr s,x
 067E 1BF1DA            leas -38,sp
 0681           ; 
 0681           ; //*****************************************************************************
 0681           ; #define  C5  47801
 0681           ; #define  C5S 45126
 0681           ; #define  D5    42589
 0681           ; #define        D5S 40193
 0681           ; #define        E5  37936
 0681           ; #define  F5    35817
 0681           ; #define  F5S 33784
 0681           ; #define        G5      31888
 0681           ; #define  G5S 30084
 0681           ; #define  A5    28409
 0681           ; #define  A5S 26824
 0681           ; #define  B5    25303
 0681           ; #define  C6    23900
 0681           ; #define  C6S 22542
 0681           ; #define  D6  21276
 0681           ; #define  D6S 20096
 0681           ; #define  E6  18825
 0681           ; #define  F6  17895
 0681           ; #define  F6S 16892
 0681           ; #define  G6    15944
 0681           ; #define  G6S 15051
 0681           ; #define  A6  14204
 0681           ; #define  A6S 13404
 0681           ; #define  B6  12658
 0681           ; 
 0681           ; 
 0681           ; #define  notes  8
 0681           ; #define  toggle 0x04
 0681           ; 
 0681           ; void Speaker ()
 0681           ; {
 0681           ;    int j;
 0681           ;    int i;
 0681           ;    unsigned int score[notes] = {C5,D5,E5,F5,G5,A5,B5,C6};
 0681 19E1EE            leay -18,x
 0684 B7C6              xgdy
 0686 CD065B            ldy #L114
 0689 34                pshx
 068A B745              tfr d,x
 068C CC0008            ldd #8
 068F           X3:
 068F 18027131          movw 2,y+,2,x+
 0693 0434F9            dbne d,X3
 0696 30                pulx
 0697           ;    unsigned int dur[notes] = {5,5,5,5,5,5,5,5};
 0697 19E1DE            leay -34,x
 069A B7C6              xgdy
 069C CD066B            ldy #L115
 069F 34                pshx
 06A0 B745              tfr d,x
 06A2 CC0008            ldd #8
 06A5           X4:
 06A5 18027131          movw 2,y+,2,x+
 06A9 0434F9            dbne d,X4
 06AC 30                pulx
 06AD           ; 
 06AD           ; //   TSCR1 = 0x90;   //Enable TCNT, fast timer flag clear
 06AD           ; //   TSCR2 = 0x83;   //Set TCNT prescaler to 8
 06AD           ;    TFLG1 = 0x20;   //Clear all TxF flags
 06AD C620              ldab #32
 06AF 7B004E            stab 0x4e
 06B2           ;    TIE  |= 0x20;   //Enable TC5 interrupt
 06B2 1C004C20          bset 0x4c,#32
 06B6           ;    TIOS |= 0x20;   //Enable OC5 function
 06B6 1C004020          bset 0x40,#32
 06BA           ;    TCTL1 = toggle; //Select toggle option as OC5 pin action
 06BA C604              ldab #4
 06BC 7B0048            stab 0x48
 06BF           ;    j = 0;
 06BF CC0000            ldd #0
 06C2 6C1E              std -2,x
 06C4           ;    dela = score[0];
 06C4 ECE1EE            ldd -18,x
 06C7 7C000E            std _dela
 06CA           ;    TC5 = TCNT + dela;      //Play the first note
 06CA                   ; vol
 06CA FC0044            ldd 0x44
 06CD F3000E            addd _dela
 06D0 7C005A            std 0x5a
 06D3 202D              bra L117
 06D5           L116:
 06D5 19E1EE            leay -18,x
 06D8 6DE1DA            sty -38,x
 06DB EC1E              ldd -2,x
 06DD 59                lsld
 06DE E3E1DA            addd -38,x
 06E1 B7C6              xgdy
 06E3 EC40              ldd 0,y
 06E5 7C000E            std _dela
 06E8 19E1DE            leay -34,x
 06EB 6DE1DA            sty -38,x
 06EE EC1E              ldd -2,x
 06F0 59                lsld
 06F1 E3E1DA            addd -38,x
 06F4 B7C6              xgdy
 06F6 EC40              ldd 0,y
 06F8 1604E4            jsr _s_pause
 06FB EC1E              ldd -2,x
 06FD C30001            addd #1
 0700 6C1E              std -2,x
 0702           L117:
 0702           ;    while(j < notes)
 0702 EC1E              ldd -2,x
 0704 8C0008            cpd #8
 0707 2DCC              blt L116
 0709           ;    {
 0709           ;       dela = score[j];     //Play the jth note
 0709           ;         s_pause(dur[j]);
 0709           ;         j++;
 0709           ;    }
 0709           ; 
 0709           ;    TIE  &= ~0x20;   //Disable TC5 interrupt
 0709 1D004C20          bclr 0x4c,#0x20
 070D           ; //   TSCR1 = 0x00;   //Turn off the Timer System
 070D           ;    PTT = 0x00;     //Blank Port T
 070D 790240            clr 0x240
 0710           ;    TCTL1 = 0;
 0710 790048            clr 0x48
 0713           ; }
 0713           L113:
 0713 B757              tfr x,s
 0715 30                pulx
 0716                   .dbline 0 ; func end
 0716 3D                rts
 0717           _OC5_ISR::
 0717           ; 
 0717           ; //******************************** OC5_ISR *************************************
 0717           ; 
 0717           ; #pragma interrupt_handler OC5_ISR()
 0717           ; void OC5_ISR ()
 0717           ; {
 0717           ;    TC5 += dela;
 0717           	; vol
 0717 FC005A            ldd 0x5a
 071A F3000E            addd _dela
 071D 7C005A            std 0x5a
 0720           ;    TFLG1 = 0x20;
 0720 C620              ldab #32
 0722 7B004E            stab 0x4e
 0725           ; }
 0725           L119:
 0725                   .dbline 0 ; func end
 0725 0B                rti
 0726           ;        fr_line -> -2,x
 0726           ;             fr -> 2,x
 0726           _linetrack::
 0726 3B                pshd
 0727 34                pshx
 0728 B775              tfr s,x
 072A 1B9E              leas -2,sp
 072C           ; //******************************************************************************
 072C           ; /*measure motor speed testing             */  
 072C           ; // void motorspeed();
 072C           ; //{
 072C           ; //   printf("%d %d\n\r",a,b); 
 072C           ;         
 072C           ;   //PWME = 0x00;
 072C           ;   // PWMDTYW1 = a;                                     //measure motor speed//
 072C           ;   // PWMDTYW3 = b;                                      //measure motor speed//
 072C           ;   // PWME = 0xaa;                                        //measure motor speed//
 072C           ;    
 072C           ;   // pause(50);                                                   //measure motor speed//
 072C           ;   // a = a+100;
 072C           ;   // b = b-100;
 072C           ; 
 072C           ; //}
 072C           ; //******************************************************************************
 072C           ; /*speedcontrol stepup testing                                         */
 072C           ;  /*
 072C           ;  void speedcontrol()
 072C           ;  
 072C           ;  {
 072C           ;       ob_led(8);
 072C           ; forward();             // set the speed control test//
 072C           ; pause(20);             // set the speed control test//
 072C           ;       
 072C           ; stop();                        // set the speed control test//
 072C           ; pause(20);             // set the speed control test//
 072C           ;        ob_led(9);      // set the speed control test//  
 072C           ; reverse();             // set the speed control test//
 072C           ; pause(20);             // set the speed control test//
 072C           ;    
 072C           ; stop();                        // set the speed control test//
 072C           ; pause(20);             // set the speed control test//
 072C           ;       
 072C           ;   } 
 072C           ; */
 072C           ; 
 072C           ; //******************************************************************************
 072C           ; /*linetrack  testing                                          */
 072C           ; void linetrack(int fr)
 072C           ; {
 072C 2061              bra L122
 072E           L121:
 072E           ;    int fr_line;
 072E           ;    while(((Intersection!=0x03)&&(fr==0))||((Backup!=0x07)&&(fr==1)))
 072E           ;    {
 072E           ;       if (fr==0)
 072E EC02              ldd 2,x
 0730 2607              bne L124
 0732           ;       {
 0732           ;          fr_line=line;
 0732 18011E000C        movw _line,-2,x
 0737           ;       }
 0737 2005              bra L125
 0739           L124:
 0739           ;       else
 0739           ;       {
 0739           ;          fr_line=Backup;
 0739 18011E0000        movw _Backup,-2,x
 073E           ;       }
 073E           L125:
 073E           ; 
 073E           ;       ob_led(line);     
 073E FC000C            ldd _line
 0741 1602CA            jsr _ob_led
 0744           ; 
 0744           ;       switch(fr_line)
 0744 EC1E              ldd -2,x
 0746 8C0001            cpd #1
 0749 2710              beq L128
 074B EC1E              ldd -2,x
 074D 8C0002            cpd #2
 0750 270E              beq L129
 0752 EC1E              ldd -2,x
 0754 8C0004            cpd #4
 0757 2715              beq L132
 0759 2016              bra L126
 075B           X5:
 075B           ;       {
 075B           L128:
 075B           ; 	  case 1:
 075B           ;          left();
 075B 1603AC            jsr _left
 075E           ; //               printf ("left\n\r");
 075E           ;            break;
 075E 2011              bra L127
 0760           L129:
 0760           ; 	  case 2:	
 0760           ;          if (fr==0)
 0760 EC02              ldd 2,x
 0762 2605              bne L130
 0764           ;                {
 0764           ;                   forward();
 0764 160382            jsr _forward
 0767           ;          }
 0767 2008              bra L127
 0769           L130:
 0769           ; 		 else
 0769           ; 		 {
 0769           ; 		    reverse();
 0769 16036D            jsr _reverse
 076C           ;          }
 076C           ; 
 076C           ;                break;
 076C 2003              bra L127
 076E           L132:
 076E           ;       case 4:		  
 076E           ; 		 right();
 076E 160397            jsr _right
 0771           ; //               printf ("right\n\r");
 0771           ;                break;
 0771           L126:
 0771           L127:
 0771           ; /*      default:
 0771           ;          if (fr==0)
 0771           ; 		 {
 0771           ; 		    forward();
 0771           ;          }
 0771           ; 		 else
 0771           ; 		 {
 0771           ; 		    reverse();
 0771           ;          }
 0771           ; */
 0771           ;       }
 0771           ; 	  
 0771           ; 	  pause(1);
 0771 CC0001            ldd #1
 0774 1604A1            jsr _pause
 0777           ;          if (fr==0)
 0777 EC02              ldd 2,x
 0779 260B              bne L133
 077B           ;                {
 077B           ;                   forward();
 077B 160382            jsr _forward
 077E           ;               s_pause(5);                             //original number 10
 077E CC0005            ldd #5
 0781 1604E4            jsr _s_pause
 0784           ;            }
 0784 2009              bra L134
 0786           L133:
 0786           ; 		 else
 0786           ; 		 {
 0786           ; 		    reverse();
 0786 16036D            jsr _reverse
 0789           ;                   s_pause(8);                         //original number 25
 0789 CC0008            ldd #8
 078C 1604E4            jsr _s_pause
 078F           ;          }
 078F           L134:
 078F           L122:
 078F FC0002            ldd _Intersection
 0792 8C0003            cpd #3
 0795 2704              beq L135
 0797 EC02              ldd 2,x
 0799 2793              beq L121
 079B           L135:
 079B FC0000            ldd _Backup
 079E 8C0007            cpd #7
 07A1 2709              beq L136
 07A3 EC02              ldd 2,x
 07A5 8C0001            cpd #1
 07A8 1827FF82          lbeq L121
 07AC           L136:
 07AC           ;    }
 07AC           ; }
 07AC           L120:
 07AC B757              tfr x,s
 07AE 30                pulx
 07AF 1B82              leas 2,sp
 07B1                   .dbline 0 ; func end
 07B1 3D                rts
 07B2           _init_ic0_1::
 07B2           ; 
 07B2           ; //******************************************************************************
 07B2           ; // IC0-1 Init
 07B2           ; 
 07B2           ; void init_ic0_1(void)
 07B2           ; {
 07B2           ; //   TSCR2 = 0x81;	   //might interfere with SOUND!!!!!!!!!!!!!!!! but in for testing
 07B2           ;    TFLG1 = 0x03;   //Clear flags
 07B2 C603              ldab #3
 07B4 7B004E            stab 0x4e
 07B7           ;    TIE  |= 0x03;   //Enable IC0+1 interrupt
 07B7 1C004C03          bset 0x4c,#3
 07BB           ;    TIOS &= 0xfc;   //make 0+1 input capture
 07BB 1D004003          bclr 0x40,#0x3
 07BF           ;    TCTL4 = 0x05;   //capture 0+1 on rising edge
 07BF C605              ldab #5
 07C1 7B004B            stab 0x4b
 07C4           ; }
 07C4           L137:
 07C4                   .dbline 0 ; func end
 07C4 3D                rts
 07C5           ;  lreg1 -> -4,x
 07C5           ;  lreg2 -> -8,x
 07C5           _IC0_ISR::
 07C5 34                pshx
 07C6 B775              tfr s,x
 07C8 1B98              leas -8,sp
 07CA           ; 
 07CA           ; void IC0_ISR (void)
 07CA           ; {
 07CA           ;    TFLG1 = 0x01;      // clear int flag 
 07CA C601              ldab #1
 07CC 7B004E            stab 0x4e
 07CF           ;    IC0_count++;
 07CF CD0008            ldy #_IC0_count
 07D2 1802401C          movw 0,y,-4,x
 07D6 1802421E          movw 2,y,-2,x
 07DA CD0829            ldy #L139
 07DD 18024018          movw 0,y,-8,x
 07E1 1802421A          movw 2,y,-6,x
 07E5 160000            jsr __ladd
 07E8 CD0008            ldy #_IC0_count
 07EB 18021C40          movw -4,x,0,y
 07EF 18021E42          movw -2,x,2,y
 07F3           ; }
 07F3           L138:
 07F3 B757              tfr x,s
 07F5 30                pulx
 07F6                   .dbline 0 ; func end
 07F6 0B                rti
 07F7           ;  lreg1 -> -4,x
 07F7           ;  lreg2 -> -8,x
 07F7           _IC1_ISR::
 07F7 34                pshx
 07F8 B775              tfr s,x
 07FA 1B98              leas -8,sp
 07FC           ; 
 07FC           ; 
 07FC           ; void IC1_ISR (void)
 07FC           ; {
 07FC           ;    TFLG1 = 0x02;      // clear int flag
 07FC C602              ldab #2
 07FE 7B004E            stab 0x4e
 0801           ;    IC1_count++;
 0801 CD0004            ldy #_IC1_count
 0804 1802401C          movw 0,y,-4,x
 0808 1802421E          movw 2,y,-2,x
 080C CD0829            ldy #L139
 080F 18024018          movw 0,y,-8,x
 0813 1802421A          movw 2,y,-6,x
 0817 160000            jsr __ladd
 081A CD0004            ldy #_IC1_count
 081D 18021C40          movw -4,x,0,y
 0821 18021E42          movw -2,x,2,y
 0825           ; }
 0825           L140:
 0825 B757              tfr x,s
 0827 30                pulx
 0828                   .dbline 0 ; func end
 0828 0B                rti
                        .area bss
 0000           _Backup::
 0000                   .blkb 2
 0002           _Intersection::
 0002                   .blkb 2
 0004           _IC1_count::
 0004                   .blkb 4
 0008           _IC0_count::
 0008                   .blkb 4
 000C           _line::
 000C                   .blkb 2
 000E           _dela::
 000E                   .blkb 2
 0010           _atd1::
 0010                   .blkb 16
 0020           _atd0::
 0020                   .blkb 16
 0030           _t_hundredths::
 0030                   .blkb 4
 0034           _t_tenths::
 0034                   .blkb 4
 0038           _t_counts::
 0038                   .blkb 4
                        .area text
 0829           L139:
 0829 00000001          .word 0,1
 082D           L63:
 082D 00000004          .word 0,4
 0831           L60:
 0831 00000000          .word 0,0
 0835           L59:
 0835 00000026          .word 0,38
 0839           L56:
 0839 00000001          .word 0,1
 083D           L36:
 083D 00004C90          .word 0,19600
 0841           L32:
 0841 00000BB8          .word 0,3000
 0845           L28:
 0845 00000398          .word 0,920
 0849           L21:
 0849 00000A8C          .word 0,2700
 084D           L17:
 084D 00004268          .word 0,17000
 0851           L13:
 0851 00000000          .word 0,0
