final.c

一个声控机器人的资料

    	    #ifdef HyperTerm
    	        transmit_strf("Stabilize\r\n");
    	    #endif                     
    	    delay_ms(2000);
    	    //wait for silence on all 3 mics
    	    reading = PINA & 0x38;
    		if (0 == reading)
    			autoState = Listen;
    		break;                 
    		
    	case Listen:
    	    #ifdef HyperTerm
    	        printf("Listen %i\r\n",sampleNum);
    	    #endif
    		//alert the user that the robot has entered the listening phase
    		PORTD.7 = TRUE;                  
    		//wait for 1st impulse
		    do {
		         ADCbuffer[0] = PINA & 0x38;
	    	} while (!ADCbuffer[0]);	
    	        	
		    //check to make sure only one mic has heard it at this point
			    do {	// may take a while (900 readings) ****
      			    ADCbuffer[index] = PINA;
      	    		index++;     
      		    	delay_us(10);                
   			    } while (index < array_size);   
               autoState = Locate;
                            
    		//find the first index when each mic heard the pulse
            for (index = 0; index < array_size; index++)
		    {
			    if ((ADCbuffer[index] & 0x08) && (index < micIndex1))
				    micIndex1 = index;                          
			    if ((ADCbuffer[index] & 0x10) && (index < micIndex2))
				    micIndex2 = index;
			    if ((ADCbuffer[index] & 0x20) && (index < micIndex3))
				    micIndex3 = index;
		    }

		    if((micIndex1<limit)&(micIndex2<limit)&(micIndex3<limit))
		    {  
                sampleNum++;
                sampleAvgMic1 += micIndex1;
                sampleAvgMic2 += micIndex2;
                sampleAvgMic3 += micIndex3;  
		    }
            
		    #ifdef HyperTerm
                printf("mic1 = %i, mic2 = %i, mic3 = %i\r\n",micIndex1, micIndex2, micIndex3);
		    #endif
		        		    
            
            if (sampleNum < 4)
                autoState = Stabilize;
            else
                autoState = Locate;
            
             PORTD.7 = FALSE;                                               
       	    //alert the user that the robot has exited the listening phase
    	    break;
    	case Locate:
    	    
    	    //averages the result
		    sampleAvgMic1 >>= 2;
            sampleAvgMic2 >>= 2;
            sampleAvgMic3 >>= 2;
    	
    	    #ifdef HyperTerm
    	        transmit_strf("Locate\r\n");
    	        delay_ms(20); 
		        printf("Average: mic1 = %i, mic2 = %i, mic3 = %i\r\n",sampleAvgMic1, sampleAvgMic2, sampleAvgMic3);
		    #endif 
		
		    if((sampleAvgMic1<limit)&(sampleAvgMic2<limit)&(sampleAvgMic3<limit))
		    {                         
			//analyze sampleAvgMic1, sampleAvgMic2, sampleAvgMic3 to determine the angle from angle_array and store in var angle
			//set an arbitrary distance to move in the distance variable
			//tLeft = (sampleAvgMic1 - sampleAvgMic2)*2;  //time difference in microseconds
			//tRight = (sampleAvgMic1 - sampleAvgMic3)*2;
			
			tLeft = (sampleAvgMic1 - sampleAvgMic3)/2;  //time difference in microseconds
			tRight = (sampleAvgMic1 - sampleAvgMic2)/2;  //time difference in microseconds 
			//find the column in angle_array with the closest time differences
			for (index = 0; index < numAngles; index++)
			{   
				if(minDiff > (charAbs(tLeft-angle_array[0][index])+charAbs(tRight-angle_array[1][index]))) 
				{                                 
					minDiff = charAbs(tLeft-angle_array[0][index])+charAbs(tRight-angle_array[1][index]);
					angle = (angle_array[2][index]);
					if(angle>180)
					    angle = angle - 360;
				}
			}

			distance = 60;
			autoState = Move;  
			//also read the distance/angle records to attempt to determine if the source was found and set the state
			//if(___)
			//	autoState = Done;
            recordMove(angle, distance);	//record the move, for the next cycle and for determining when the system is done  
            
          }
          //if don't hear evenly, rotate to optimize listening
          else
          {
               recordMove(30,0);
               autoState = Move;   
          }                                
          
          sampleNum = 0;
          sampleAvgMic1 = 0;
          sampleAvgMic2 = 0;
          sampleAvgMic3 = 0;      
          
          // End of Angie's ***Stickers***
    		break;
    	case Move:
    	    #ifdef HyperTerm
    	        transmit_strf("Move\r\n");
    	    #endif
    		rotateMoveCar(angle_record[buffer_length-1], distance_record[buffer_length-1]);
    		autoState = Stabilize;
    		break;
    	case Done:
    	    done();
    	    break;
    		    
        //Unaccounted for case  
        default:
            #ifdef HyperTerm
                transmit_strf("autoControl()\r\n");
            #endif
            error();
    }
}

void recordMove(signed int ang, signed int dist) {
    unsigned int index;
    #ifdef HyperTerm
        transmit_strf("Move: Angle = ");
        delay_ms(ms_delay);
        sprintf(t_buffer, "%i", ang);
        enable_transmit_interrupt();
        transmit_strf(", Distance = ");
        delay_ms(ms_delay);
        sprintf(t_buffer, "%i", dist);
        enable_transmit_interrupt();
        transmit_strf("\r\n");
    #endif    
    for (index = 0; index < buffer_length - 1; index++) {
        distance_record[index] = distance_record[index+1];
        angle_record[index] = angle_record[index+1];
    }
    distance_record[index] = dist;
    angle_record[index] = ang;
}

// *********************************************************
// RECEIVE/TRANSMIT FUNCTIONS 
// *********************************************************
   
//********************************************************** 

//set up receive interrupts 
void enable_receive_interrupt() 
{
    r_ready=0;    //get ready to receive
    //r_index=0;    //reset buffer counter
    UCSRB.7=1;    //turn on ISR
}

//********************************************************** 
//set up transmit interrupts   
void enable_transmit_interrupt() 
{
    t_ready=0;
    t_index=0;
    if (t_buffer[0] != '\0') 
    {
  	    putchar(t_buffer[0]);
  	    UCSRB.5=1;
    }
}



// *********************************************************
// INITIALIZATION 
// *********************************************************
//Set it all up
void initialize()
{     
    //set up ports 
    DDRB 		 = 0xff;            	//PORTB for car wheels 
    PORTB 		 = 0;                          
    DDRA 		 = 0x00;            	//PINA for speaker data
    PORTA 		 = 0;
    DDRC 		 = 0b00000011;      	//PORTC for camera input
    DDRD.7 	 	 = 1;             		//PORTD for LED 
    PORTD.7 	 = 1;
        
    //serial setup for debugging using printf, etc.     
    UCSRB 		 = 0x18;                 //enable receive/transmit
    UBRRL 		 = 103;                  //set baud rate to 9600
    t_ready		 = TRUE;
    t_index		 = 0;
    r_ready		 = TRUE;
    r_index		 = 0;
    
    //set up timer 0     
    OCR0 		 = 124;	   				//0.5 mSec
    TIMSK 	 	 = 2;	   				//turn on timer 0 cmp-match ISR 
    TCCR0 		 = 0b00001011;			//prescalar to 64  and Clr-on-match        

    //init the task timers
    timeUpdate   = tUpdate; 
    timePWM 	 = tPWM;    
    
    //set car control variables
    servo1 		 = 0;             		//turn PWM off
    servo2 		 = 0;
    servo3 		 = 0;
    servoState   = Idle; 				//the servos are Idle
    
    //set car movement variables
    rotateTime   = 0;         			//turn off car control timers
    moveTime 	 = 0;
    waitTime 	 = 0;
    scaledMove   = romScaledMove;   	//CALIBRATED SCALED VARIABLES
    scaledRotate = romScaledRotate;   	//CALIBRATED SCALED VARIABLES
    
    //set autonomous variables
    autoState   = Stabilize;     
    //autoState = Controlled;             //not in autonomous mode  
    
    sampleNum   = 0;
    sampleAvgMic1   = 0;
    sampleAvgMic2   = 0;
    sampleAvgMic3   = 0;
          
    #ifdef HyperTerm
        autoState = Controlled;
        printf("Starting Control Mode <mode>,<angle>,<distance>,\r\n");
    #endif
    //DEBUG
    #if DEBUG
        autoTurnCount = 0;
        autoState = Stabilize; 
    #endif
    	
    
    //enable the interrupt service routine
    #asm("sei");
    
    //enable receive interrupts
    enable_receive_interrupt(); 
}  

//********************************************************** 
//set up transmit interrupts 
void parseCommandASCII(unsigned char command_buffer[], unsigned char* usr_mode,
          signed int* usr_angle, signed int* usr_dist, unsigned char deliminator)
{
    unsigned char index, arg_index = 1, cmd_index[3];
    cmd_index[0] = 0;
    for (index = 0; command_buffer[index] != '\0' && index < buffer_length && arg_index < 3; index++)
    {
        if (command_buffer[index] == deliminator)
        {
            command_buffer[index] = '\0';
            cmd_index[arg_index] = index + 1;
            arg_index++;
        }
        
    }
    *usr_mode = (unsigned char) atoi(&command_buffer[cmd_index[0]]);
    *usr_angle = atoi(&command_buffer[cmd_index[1]]);
    *usr_dist = atoi(&command_buffer[cmd_index[2]]);
}

// *********************************************************
// MAIN PROGRAM 
// *********************************************************     

//Entry point and task scheduler loop
void main()
{                
    initialize();
    //main task scheduler loop -- never exits!    
    //Suuuppperrr Looooooop!
    while(TRUE)
    {    
        
        //check system
        if (timeUpdate == 0)	
            updateSystem();
                  
        //check wheels 
        //turn high when...
        if (timePWM == servo1-1)      //check back wheel
            PORTB.0 = 1;   
            
        if (timePWM == servo2-1)      //check left wheel
            PORTB.1 = 1;
            
        if (timePWM == servo3-1)      //check right wheel
            PORTB.2 = 1;

        //turn low when...   
        if (timePWM == 0) 
        	updateServos();
        	

    }   
}