sonar.c

老外开发的机器人的底层单片机代码。比较有参考价值哦！

//  sonar     sonar.c					// Rev 9/1/05

//Copyright (C) 2005 Alex Brown	[e-mail]
//This program is free software; See license at the end of this file for details.

/*
	Reads up to 7 SRF04 sonars attached to SON0 through SON6
	    Sonar	  Trigger pin	  Echo pin
		 SON0	  	PortA 7		    TC3											
		 SON1	  	PortA 6		    TC4											
		 SON2	  	PortA 5		    TC5											
		 SON3	  	PortA 4		    TC6	
		 SON4	  	PortK 4		    TC0											
		 SON5	  	PortK 5		    TC1											
		 SON6	  	PortK 7		    TC2	
		 
	The number of sonars to read is specified by the NumSonars variable set by the
	laptop.  The number can vary from zero to 7.  Sonars and RC servos share the
	same input ports. Sonars must start at RC0 and go up. RCservos start when 
	servos end and continue up to RC6.
	
	Reads one sonar at a time using two computation cycles for each. Data is 
	updated every 32 msec times the number of sonars.(e.g. 4 sonars take 128
	msec to update.	
	
	This is to ensure sonars don't interfere with each other.	 	
		 
	on return, RC_Sonar[n] = 0 if invalid, else is distance in millimeters.	
	
  */
  
#include "Microcontroller.h"		//for registers & configuration data

int NumSonars = 4;		// initial value.  May be changed by laptop

//inputs
extern int time;  //used to cycle through sonar readings
  
//exported distance values 
int RC_Sonar[7];
	
//-------------------------------------------------------------------------
void sonar()
  { int i; 				      //delay loop counter
    static int casenum = -1;  //index to switch
    static int SonarStart;	  //trigger pulse time
	static int SonarEnd;	  //echo pulse time

	if(NumSonars > 0)
	{
	   casenum++;
	   if(casenum >= (NumSonars * 2)) casenum = 0;
	}
	else casenum = -1;
	
    switch (casenum)	                      //time modulus 8
     { case 0:						//Sonar 0
	     PORTA |= 0x80;						  //turn on trigger pulse
		 for (i = 0; i<30; i++) ;			  //20 microsecond delay
		 PORTA &= ~0x80;	   				  //turn off trigger
		 SonarStart = TCNT;					  //save the start time
		 TFLG1 |= 0x08;					  	  //reset capture flag
		 break;
		 
	   case 1:
	     if (TFLG1 & 0x08)					  //if time captured,
		  { SonarEnd = TC3;					  //store echo time
			if (SonarEnd > SonarStart)
			   RC_Sonar[0] = SonarEnd - SonarStart;
			else		  			   		  //compensate for rollover
			   RC_Sonar[0] = 30000 - SonarStart +SonarEnd;
			     
			RC_Sonar[0] = (long)RC_Sonar[0]*100/867 -46; //convert to dist in mm
		  }
		 else RC_Sonar[0] = 0;  //0 means invalid (time not captured) 
		 break;
		 
	   case 2:					  //Sonar 1
	     PORTA |= 0x40;						  //turn on trigger pulse
		 for (i = 0; i<30; i++) ;			  //20 microsecond delay
		 PORTA &= ~0x40;	   				  //turn off trigger
		 SonarStart = TCNT;					  //save the start time
		 TFLG1 |= 0x10;					  	  //reset capture flag
		 break;
		 
	   case 3:
	     if (TFLG1 & 0x10)					  //if time captured,
		  { SonarEnd = TC4;					  //store echo time
			if (SonarEnd > SonarStart)
			   RC_Sonar[1] = SonarEnd - SonarStart;
			else		  			   		  //compensate for rollover
			   RC_Sonar[1] = 30000 - SonarStart +SonarEnd;
			     
			RC_Sonar[1] = (long)RC_Sonar[1]*100/867 -46; //convert to dist in mm
		  }
		 else RC_Sonar[1] = 0;  //0 means invalid (time not captured) 
		 break;
		 

       case 4:						//Sonar 2
	     PORTA |= 0x20;						  //turn on trigger pulse
		 for (i = 0; i<30; i++) ;			  //20 microsecond delay
		 PORTA &= ~0x20;	   				  //turn off trigger
		 SonarStart = TCNT;					  //save the start time
		 TFLG1 |= 0x20;					  	  //reset capture flag
		 break;
		 
	   case 5:
	     if (TFLG1 & 0x20)					  //if time captured,
		  { SonarEnd = TC5;					  //store echo time
			if (SonarEnd > SonarStart)
			   RC_Sonar[2] = SonarEnd - SonarStart;
			else		  			   		  //compensate for rollover
			   RC_Sonar[2] = 30000 - SonarStart +SonarEnd;
			     
			RC_Sonar[2] = (long)RC_Sonar[2]*100/867 -46; //convert to dist in mm
		  }
		 else RC_Sonar[2] = 0;  //0 means invalid (time not captured) 
		 break;
		 
	   case 6:						//Sonar 3
	     PORTA |= 0x10;						  //turn on trigger pulse
		 for (i = 0; i<30; i++) ;			  //20 microsecond delay
		 PORTA &= ~0x10;	   				  //turn off trigger
		 SonarStart = TCNT;					  //save the start time
		 TFLG1 |= 0x40;					  	  //reset capture flag
		 break;
		 
	   case 7:
	     if (TFLG1 & 0x40)					  //if time captured,
		  { SonarEnd = TC6;					  //store echo time
			if (SonarEnd > SonarStart)
			   RC_Sonar[3] = SonarEnd - SonarStart;
			else		  			   		  //compensate for rollover
			   RC_Sonar[3] = 30000 - SonarStart +SonarEnd;
			     
			RC_Sonar[3] = (long)RC_Sonar[3]*100/867 -46; //convert to dist in mm
		  }
		 else RC_Sonar[3] = 0;  //0 means invalid (time not captured) 
		 break;
		 
	   case 8:						//Sonar 4
	     PORTK |= 0x10;						  //turn on trigger pulse
		 for (i = 0; i<30; i++) ;			  //20 microsecond delay
		 PORTK &= ~0x10;	   				  //turn off trigger
		 SonarStart = TCNT;					  //save the start time
		 TFLG1 |= 0x01;					  	  //reset capture flag
		 break;
		 
	   case 9:
	     if (TFLG1 & 0x01)					  //if time captured,
		  { SonarEnd = TC0;					  //store echo time
			if (SonarEnd > SonarStart)
			   RC_Sonar[4] = SonarEnd - SonarStart;
			else		  			   		  //compensate for rollover
			   RC_Sonar[4] = 30000 - SonarStart +SonarEnd;
			     
			RC_Sonar[4] = (long)RC_Sonar[4]*100/867 -46; //convert to dist in mm
		  }
		 else RC_Sonar[4] = 0;  //0 means invalid (time not captured) 
		 break;
		 
	   case 10:						//Sonar 5
	     PORTK |= 0x20;						  //turn on trigger pulse
		 for (i = 0; i<30; i++) ;			  //20 microsecond delay
		 PORTK &= ~0x20;	   				  //turn off trigger
		 SonarStart = TCNT;					  //save the start time
		 TFLG1 |= 0x02;					  	  //reset capture flag
		 break;
		 
	   case 11:
	     if (TFLG1 & 0x02)					  //if time captured,
		  { SonarEnd = TC1;					  //store echo time
			if (SonarEnd > SonarStart)
			   RC_Sonar[5] = SonarEnd - SonarStart;
			else		  			   		  //compensate for rollover
			   RC_Sonar[5] = 30000 - SonarStart +SonarEnd;
			     
			RC_Sonar[5] = (long)RC_Sonar[5]*100/867 -46; //convert to dist in mm
		  }
		 else RC_Sonar[5] = 0;  //0 means invalid (time not captured) 
		 break;
		 
	   case 12:						//Sonar 6
	     PORTK |= 0x80;						  //turn on trigger pulse
		 for (i = 0; i<30; i++) ;			  //20 microsecond delay
		 PORTK &= ~0x80;	   				  //turn off trigger
		 SonarStart = TCNT;					  //save the start time
		 TFLG1 |= 0x04;					  	  //reset capture flag
		 break;
		 
	   case 13:
	     if (TFLG1 & 0x04)					  //if time captured,
		  { SonarEnd = TC2;					  //store echo time
			if (SonarEnd > SonarStart)
			   RC_Sonar[6] = SonarEnd - SonarStart;
			else		  			   		  //compensate for rollover
			   RC_Sonar[6] = 30000 - SonarStart +SonarEnd;
			     
			RC_Sonar[6] = (long)RC_Sonar[6]*100/867 -46; //convert to dist in mm
		  }
		 else RC_Sonar[6] = 0;  //0 means invalid (time not captured) 
		 break;
    }                //end of switch
//printf("So   %8d  %8d  %8d  %8d\n",
//			   		RC_Sonar[0],RC_Sonar[1],RC_Sonar[2],RC_Sonar[3]);	
  }

//  OPEN SOURCE SOFTWARE LICENSE
/* Permission is hereby granted, free of charge, to any person obtaining a copy 
of this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to use, 
copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 
Software, and to permit persons to whom the Software is furnished to do so, 
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all 
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/