final.c

一个声控机器人的资料

#include "final.h"              

// *********************************************************
// INTERRUPTS 
// *********************************************************    

//**********************************************************
//Timer 0 Overflow
interrupt [TIM0_COMP] void timer0_compare()
{   
    //Decrement the times if they are not already zero
    if (timeUpdate > 0)	
        timeUpdate--;
        
    if (timePWM > 0)
        timePWM--;
}  

//**********************************************************
//UART character-ready ISR
interrupt [USART_RXC] void uart_rec()
{
	r_char=UDR;    //get a char
	UDR=r_char;    //then print it
	//build the input string
	if (r_char != '\r')
	  if (r_char != 0x08) //don't record backspaces 
	    r_buffer[r_index++] = r_char; //add string to the buffer, and shift point over
	  //handle backspace            
	  else 
	    r_index--;
	else
	{
		putchar('\n');  			//use putchar to avoid overwrite
		r_buffer[r_index]='\0';    	//insert null character
		r_ready=TRUE;				//signal cmd processor
		UCSRB.7=0;   				//stop rec ISR
		r_index = 0;
	}
}

//**********************************************************
//UART xmit-empty ISR (transmit Data Register Empty, can put a new char in the transmit buffer)
interrupt [USART_DRE] void uart_send()
{
	t_char = t_buffer[++t_index];
	if (t_char == 0) 	
	{
		UCSRB.5=0; //kill isr 
		t_ready=TRUE; //transmit done 
	}
	else
	    UDR = t_char ;     //send the char 
}

//**********************************************************
//trasnmit a string from flash using interrupt driven serial communication
void transmit_strf(unsigned char flash *str)
{   
    while(!t_ready);
    strncpyf(t_buffer,str,buffer_length-1);
    enable_transmit_interrupt();
}

// *********************************************************
// CAR CONTROL 
// *********************************************************  
  
//**********************************************************          
//updateSystem - checks for changes in inputs
void updateSystem()    
{   
    signed int distance, angle;
    unsigned char user_mode; 
    
    timeUpdate = tUpdate;  		           //reset the task timer
    
    //if new input from PDA...    
    if (r_ready)    
    {   
        #ifdef HyperTerm
            parseCommandASCII(r_buffer, &user_mode, &angle, &distance, ',');
            //angle = angle << 1;
            printf("Command: mode = %i, angle = %i, dist = %i\r\n", user_mode, angle, distance); 
        #else
            user_mode = r_buffer[0];              //store new user input
            angle = ((int)r_buffer[1])<<1;
            distance = r_buffer[2];
        #endif
        enable_receive_interrupt();     //reset receive interrupt
        resetServors();                 //reset the servoState
        
        switch (user_mode)
        {   
            //we go into controlled mode
            case UsrControl:
                #ifdef HyperTerm
	                transmit_strf("User Control\r\n");
                #endif
                autoState = Controlled; //make sure autonomous mode is off
                rotateMoveCar(angle, distance);   //move car
                break;
            
            //we set the car to search for sound!
            case UsrListen:
                #ifdef HyperTerm
	                transmit_strf("UserListen\r\n");
                #endif
                autoState = Stabilize;          //turn on autonomous state machine
                autoTurnCount = 0;      
                break;
            
            //we store to rom the new calibrations, and set the,
            case UsrEEPROMUpdate:
                #ifdef HyperTerm
	                transmit_strf("User EEProm Update\r\n");
                #endif
                romScaledRotate = angle;
                scaledRotate = angle;
                romScaledMove = distance;
                scaledMove = distance;
                break;
            
            //if we don't have one of these cases ERROR!!!
            default:
                #ifdef HyperTerm
                    transmit_strf("updateSystem()\r\n");
                    delay_ms(50);
                #endif
                error();
        }     
    }  
    
    //if in autonomous mode, run state machine
    if (autoState != Controlled) 
        autoControl();     
}  

// *********************************************************
// CAR MOVEMENT 
// *********************************************************

//**********************************************************
//run function to rotate car   
void rotateMoveCar(signed int angle, signed int distance)
{   
	//if soemthing it telling the car to rotate 
	//while it's not idle that's BAAAADDDD
	if(servoState != Idle)
	{
	    #ifdef HyperTerm
	        transmit_strf("rotateMoveCar()\r\n");
	    #endif
		error();            
	}

    //calculate time needed to turn
    rotateTime = (scaledRotate*intAbs(angle))>>3;  
    
    //check which direction to turn
    if (angle < 0)
    {
        servo1 = 6;    //turn all wheels to right
        servo2 = 6;
        servo3 = 6;
    }
    else if (angle > 0)
    {
        servo1 = 2;    //turn all wheels to left
        servo2 = 2;
        servo3 = 2;
    } 
    else
    {
    	servo1 = 0;    //turn all wheels off
        servo2 = 0;
        servo3 = 0;
        rotateTime = 0; 
    }
    moveCar(distance);
    servoState = Rotating;
}

//**********************************************************
//run function to move car   
void moveCar(signed int distance)
{                   
    //calculate time needed to move
    moveTime = (scaledMove*intAbs(distance))>>3;
    
    //move front two wheels forward
    if (distance > 0)
    	moveForeward = 1;
    
    //move front two wheels backward
    else if (distance < 0)
    	moveForeward = 2;
    
    else 
    {
    	moveForeward = 0;
    	moveTime = 0;
    }  

}

//**********************************************************
//update the servo state machine
void updateServos()
{   
	PORTB = 0;    		//moo
	timePWM = tPWM;		//reset the PWM counter
	
	//run the proper state machine code    
	switch (servoState)
	{
		//nothing to see here.... 
	    case Idle:
	        break;
	    
	    //we just wait here a while till we go into the foreward state
	    case Waiting:
	        if (waitTime > 0)
	            waitTime--;
	            
	        else
	        {    
	        	if(0 == moveForeward)
	        	{   
	        		servo1 = 0;
	        		servo2 = 0;
        			servo3 = 0;
	        	}
	        	else if(1 == moveForeward)
	        	{   
	        		servo1 = 0;
	        	 	servo2 = 6;
        			servo3 = 2;
	        	}
	        	else if(2 == moveForeward)
	        	{     
	        		servo1 = 0;
	        		servo2 = 2;
        			servo3 = 6;
	        	}
	            servoState = Foreward;
	        }   
	            
	        break;
	    
	    //we keep on decreasing rotateTime till it's 0, 
	    //then turn off servos and go into waiting mode   
	    case Rotating:
	        if (rotateTime > 0 && rotateTime < 270)
	            rotateTime--; 
	        
	        else
	        {
	            servo1 = 0;
	            servo2 = 0;
	            servo3 = 0;
	            waitTime = 10;
	            servoState = Waiting;
	        }
	         
	        break;
	   
	   	//we keep on decreasing moveTime till it's 0,
	   	//then we turn off servos and go into idle mode 
	    case Foreward:  
	        if ((moveTime > 0)&(moveTime<300))
	            moveTime--;     
	            
	        else
	        {
	            servo1 = 0;
	            servo2 = 0;
	            servo3 = 0;
	            servoState = Idle;
	        }
	        break;
	        
	    default:
            #ifdef HyperTerm
                transmit_strf("updateServos()\r\n");
            #endif
	        error();
	}
}

void resetServors()
{
    servoState = Idle;
    servo1 = 0;
    servo2 = 0;
    servo3 = 0;
    PORTB = 0;
}			

// *********************************************************
// AUTONOMOUS MODE 
// *********************************************************
   
//********************************************************** 

void autoControl()
{         
    unsigned int index = 0, micIndex1 = array_size, micIndex2 = array_size, micIndex3 = array_size;
    unsigned char tLeft, tRight, minDiff=1200, reading;
    signed int distance, angle = 0; 
    
    if(servoState != Idle)
        return;
    
    switch (autoState) 
    {
    	case Stabilize: