//***************************************************************************** // Name: 2007 Line Track code // Vers: 0.1 // Last Revised: 1/27/07 10:16pm //***************************************************************************** #include #include //define 16 bit PWM registers #define PWMPERW1 _LP(0xB4) #define PWMPERW3 _LP(0xB6) #define PWMPERW5 _LP(0xB8) #define PWMPERW7 _LP(0xBA) #define PWMDTYW1 _LP(0xBC) #define PWMDTYW3 _LP(0xBE) #define PWMDTYW5 _LP(0xC0) #define PWMDTYW7 _LP(0xC2) // Function Prototypes void init_pwm(void); void init_timer(void); void ob_led(int); void pause(int); void s_pause(int); void init_tof(void); void init_atd0(void); void init_atd1(void); void Speaker(void); void stop(void); void forward(void); void reverse(void); void left(void); void right(void); void motorspeed(void); void speedcontrol(void); void linetrack(int); //General #define CLI asm("CLI") #define SEI asm("SEI") //TOF Interrupt Setup void TOF_isr(void); #pragma interrupt_handler TOF_isr #pragma abs_address: 0x3e5e void (*IRQ_TOF_vector[])()={TOF_isr}; #pragma end_abs_address //ATD0 Interrupt Setup void ATD0_Interrupt(); #pragma interrupt_handler ATD0_Interrupt() #pragma abs_address: 0x3E52 void (*IRQ_ATD0_vector[])()={ATD0_Interrupt}; #pragma end_abs_address //ATD1 Interrupt Setup void ATD1_Interrupt(); #pragma interrupt_handler ATD1_Interrupt() #pragma abs_address: 0x3E50 void (*IRQ_ATD1_vector[])()={ATD1_Interrupt}; #pragma end_abs_address //OC5 Interrupt Setup void OC5_ISR(); #pragma interrupt_handler OC5_ISR() #pragma abs_address: 0x3E64 void (*IRQ_OC5_vector[])()={OC5_ISR}; #pragma end_abs_address //IC0 Interrupt Setup void IC0_ISR(); #pragma interrupt_handler IC0_ISR() #pragma abs_address: 0x3E6E void (*IRQ_IC0_vector[])()={IC0_ISR}; #pragma end_abs_address //IC1 Interrupt Setup void IC1_ISR(); #pragma interrupt_handler IC1_ISR() #pragma abs_address: 0x3E6C void (*IRQ_IC1_vector[])()={IC1_ISR}; #pragma end_abs_address //declare global variable long int t_counts; long int t_tenths; long int t_hundredths; unsigned int atd0[8]; unsigned int atd1[8]; int dela; int line; const line_threshold = 90; unsigned int IC0_last; unsigned int IC1_last; unsigned int IC0_speed; unsigned int IC1_speed; int Intersection; const Intersection_threshold = 60; int Backup; const Backup_threshold = 90; void main(void) { int a,b; init_pwm(); init_tof(); init_atd0(); init_atd1(); // init_ic0_1(); pause(1); ATD0CTL5 = 0x30; //start atd's ATD1CTL5 = 0x30; pause(1); //pause to allow atd to read first time; ob_led(15); while(atd0[3] > 10); //wait for S2 switch to be pressed ob_led(14); pause(50); forward(); pause(8); /* while(1) { reverse(); ob_led(13); pause(10); linetrack(1); ob_led(12); stop(); pause(20); Speaker(); } */ //while(1) //{ linetrack(0); //linetrack to T ob_led(11); forward(); //center robot on line pause(5); stop(); pause(5); left(); //turn till sensor on line while(line==0) { ob_led(line); } linetrack(0); //linetrack to 4 way intersection ob_led(10); forward(); //center robot on line pause(5); stop(); pause(8); stop(); pause(5); left(); //turn till sensor on line heading towards pause(5); //ball request zone while(line==0) { ob_led(line==0); //(line) } stop(); pause(5); linetrack(0); // get to ball request zone stop(); pause(5); reverse(); // head back to 4 way intersection pause(5); linetrack(1); stop(); // at 4 way pause(5); reverse(); pause(10); //Checking Line Track line = 0; if (atd1[0] < line_threshold) { line += 0x1; } if (atd1[1] < line_threshold) { line += 0x2; } if (atd1[2] < line_threshold) { line += 0x4; } linetrack(1); //head to rebound zone and stop stop(); pause(10); Speaker(); // printf ("line = %d %d\n\r",atd1[7],atd1[6]); //} while(1); } // Display value passed to function on on board 7-seg LED of MiniDragon Board void ob_led(int t) { int out[16] = {0x3f,0x06,0x5b,0x4f,0x66,0x6d,0x7d,0x07,0x7f,0x6f,0x77,0x7c, 0x39,0x5e,0x79,0x71}; DDRH=0xff; PTH=out[t]; } // Set up 4-16bit PWM void init_pwm() { PWMTST = 0x00; // turn off all PWM special Modes PWMCAE = 0x00; // PWM left aligned output mode PWMCTL = 0xf0; // Use Concatenated Mode (16-bit) PWMPRCLK = 0x33; // E/8 for A and B PWMCLK = 0x00; // PW Chan 1, 5 clock A // PW chan 3, 7 clock B PWMPOL= 0xaa; // High Polarity for all channels DDRP= 0xaa; // Set Port P to output PWMPERW1 = 60000; //stop (7.5%) all motors PWMDTYW1 = 4500; //<- in theory this value should be 4500 PWMPERW3 = 60000; PWMDTYW3 = 4500; //<- in theory this value should be 4500 PWMPERW5 = 60000; PWMDTYW5 = 4500; PWMPERW7 = 60000; PWMDTYW7 = 4500; PWME= 0xaa; // Enable ALL PWM Channels } // Stop void stop() { PWME = 0x00; PWMDTYW1 = 4500; //4400 PWMDTYW3 = 4500; //4600 PWME = 0xaa; } // reverse void reverse() { PWME = 0x00; PWMDTYW1 = 4000; //half full speed forward 4876// PWMDTYW3 = 5000; //half full speed reverse 4125// PWME = 0xaa; } // forward void forward() { PWME = 0x00; PWMDTYW1 = 5000; //half full speed reverse// PWMDTYW3 = 4000; //half full speed forward// PWME = 0xaa; } // right void right() { PWME = 0x00; PWMDTYW1 = 5000; //orignal speed 5250// PWMDTYW3 = 5000; //orignal speed 5250// PWME = 0xaa; } // left void left() { PWME = 0x00; PWMDTYW1 = 4000; //orignal speed 3750// PWMDTYW3 = 4000; //orignal speed 3750// PWME = 0xaa; } //Initialize Timer Overflow void init_tof() { SEI; TSCR1 = 0x80; //turn on Timer TFLG2 = 0x80; //clear TOF flag TSCR2 = 0x80; //turn on TOF interrupt CLI; } //Timer Overflow Counter (ISR) void TOF_isr() { t_counts++; if ((t_counts%38)==0) { t_tenths++; } if ((t_counts%4)==0) { t_hundredths++; } TFLG2 = 0x80; } //pause for pause_time*0.1 sec void pause(int pause_time) { long int end_time; end_time = t_tenths + pause_time; // printf ("%ld = %ld + %d\n\r",end_time,t_tenths,pause_time); while(end_time>t_tenths) { } } //pause for pause_time*0.01 sec void s_pause(int pause_time) { long int end_time; end_time = t_hundredths + pause_time; // printf ("%ld = %ld + %d\n\r",end_time,t_tenths,pause_time); while(end_time>t_hundredths) { } } // ******************* Initialize ATD ******************* void init_atd0() { SEI; ATD0CTL2 = 0xC2; ATD0CTL3 = 0x40; ATD0CTL4 = 0xE7; CLI; } /********************************************************************/ /* This code is primarily taken from example 7.3.5 in the text */ /* and then converted to act as an ISR */ /********************************************************************/ void ATD0_Interrupt() { /*print results to PC screen for testing */ atd0[0]= ATD0DR0H; atd0[1]= ATD0DR1H; atd0[2]= ATD0DR2H; atd0[3]= ATD0DR3H; atd0[4]= ATD0DR4H; atd0[5]= ATD0DR5H; atd0[6]= ATD0DR6H; atd0[7]= ATD0DR7H; } // ******************* Initialize ATD ******************* void init_atd1() { SEI; ATD1CTL2 = 0xC2; ATD1CTL3 = 0x40; ATD1CTL4 = 0xE7; CLI; } /********************************************************************/ /* This code is primarily taken from example 7.3.5 in the text */ /* and then converted to act as an ISR */ /********************************************************************/ void ATD1_Interrupt() { /*print results to PC screen for testing */ atd1[0]= ATD1DR0H; atd1[1]= ATD1DR1H; atd1[2]= ATD1DR2H; atd1[3]= ATD1DR3H; atd1[4]= ATD1DR4H; atd1[5]= ATD1DR5H; atd1[6]= ATD1DR6H; atd1[7]= ATD1DR7H; //Checking Line Track line = 0; if (atd1[0] < line_threshold) { line += 0x1; } if (atd1[1] < line_threshold) { line += 0x2; } if (atd1[2] < line_threshold) { line += 0x4; } //Checking Intersection Intersection = 0; if (atd1[6] < Intersection_threshold) { Intersection += 0x1; } if (atd1[7] < Intersection_threshold) { Intersection += 0x2; } //Checking Backup Backup = 0; if (atd1[3] < Backup_threshold) { Backup += 0x1; } if (atd1[4] < Backup_threshold) { Backup += 0x2; } if (atd1[5] < Backup_threshold) { Backup += 0x4; } // printf("%x %x %x %x\n\r", atd1[0], atd1[1], atd1[2], line); } //***************************************************************************** #define C5 47801 #define C5S 45126 #define D5 42589 #define D5S 40193 #define E5 37936 #define F5 35817 #define F5S 33784 #define G5 31888 #define G5S 30084 #define A5 28409 #define A5S 26824 #define B5 25303 #define C6 23900 #define C6S 22542 #define D6 21276 #define D6S 20096 #define E6 18825 #define F6 17895 #define F6S 16892 #define G6 15944 #define G6S 15051 #define A6 14204 #define A6S 13404 #define B6 12658 #define notes 8 #define toggle 0x04 void Speaker () { int j; int i; unsigned int score[notes] = {C5,D5,E5,F5,G5,A5,B5,C6}; unsigned int dur[notes] = {5,5,5,5,5,5,5,5}; // TSCR1 = 0x90; //Enable TCNT, fast timer flag clear // TSCR2 = 0x83; //Set TCNT prescaler to 8 TFLG1 = 0x20; //Clear all TxF flags TIE |= 0x20; //Enable TC5 interrupt TIOS |= 0x20; //Enable OC5 function TCTL1 = toggle; //Select toggle option as OC5 pin action j = 0; dela = score[0]; TC5 = TCNT + dela; //Play the first note while(j < notes) { dela = score[j]; //Play the jth note s_pause(dur[j]); j++; } TIE &= ~0x20; //Disable TC5 interrupt // TSCR1 = 0x00; //Turn off the Timer System PTT = 0x00; //Blank Port T TCTL1 = 0; } //******************************** OC5_ISR ************************************* #pragma interrupt_handler OC5_ISR() void OC5_ISR () { TC5 += dela; TFLG1 = 0x20; } //****************************************************************************** /*measure motor speed testing */ // void motorspeed() //{ // printf("%d %d\n\r",a,b); //PWME = 0x00; // PWMDTYW1 = a; //measure motor speed// // PWMDTYW3 = b; //measure motor speed// // PWME = 0xaa; //measure motor speed// // pause(50); //measure motor speed// // a = a+100; // b = b-100; //} //****************************************************************************** /*speedcontrol stepup testing */ /* void speedcontrol() { ob_led(8); forward(); // set the speed control test// pause(20); // set the speed control test// stop(); // set the speed control test// pause(20); // set the speed control test// ob_led(9); // set the speed control test// reverse(); // set the speed control test// pause(20); // set the speed control test// stop(); // set the speed control test// pause(20); // set the speed control test// } */ //****************************************************************************** /*linetrack testing */ void linetrack(int fr) { int fr_line; while(((Intersection!=0x03)&&(fr==0))||((Backup!=0x07)&&(fr==1))) { if (fr==0) { fr_line=line; } else { fr_line=Backup; } ob_led(fr_line); switch(fr_line) { case 1: left(); // printf ("left\n\r"); break; case 2: if (fr==0) { forward(); } else { reverse(); } break; case 4: right(); // printf ("right\n\r"); break; /* default: if (fr==0) { forward(); } else { reverse(); } */ } pause(1); if (fr==0) { forward(); s_pause(5); //original number 10 } else { reverse(); s_pause(8); //original number 25 } } } //****************************************************************************** // IC0-1 Init void init_ic0_1(void) { // TSCR2 = 0x81; //might interfear with SOUND!!!!!!!!!!!!!!!! but in for testing TFLG1 = 0x03; //Clear flags TIE |= 0x03; //Enable IC0+1 interrupt TIOS &= 0xfc; //make 0+1 input capture TCTL4 = 0x05; //capture 0+1 on rising edge } void IC0_ISR (void) { TFLG1 = 0x01; // clear int flag IC0_speed=TC0-IC0_last; IC0_last=TC0; } void IC1_ISR (void) { TFLG1 = 0x02; // clear int flag IC1_speed=TC1-IC1_last; IC1_last=TC1; }