Ultraschall Entfernungs-Messung

SRF04 Beispiel

//
// SFR04 ultrasonic distance measuring
//
// Mikk Leini
// Department of Mechatronics
// Tallinn University of Technology
//
// NB!
// CPU clock has to be 14745600 hz
//
// 2009
//
 
// Include avrlibc
#include <avr/io.h>
#include <avr/interrupt.h>
 
// Include common library
#include <stdlib.h>
#include "pin.h"
#include "lcd.h"
 
// Configuration
#define INTEGRATION_STEP 5
 
// Math operations
#define MIN(a, b)   ((a) < (b) ? (a) : (b))
#define MAX(a, b)   ((a) > (b) ? (a) : (b))
#define INTEGRATE(value, new_value, n)  value = (value * (n - 1) + new_value) / n;
 
// Configure pins
#define LEDR                       PORTPIN(C, 5)
#define US_TRIGGER                 PORTPIN(E, 5)
#define US_ECHO                    PORTPIN(E, 4)
 
// Ultrasonic data structure
struct
{
	unsigned char state;
	unsigned long front;
	unsigned short result;
} ultrasonic;
 
// 16 bit timestamp
volatile unsigned long timestamp = 0;
 
 
//
// Start measuring timer
//
void clock_init(void)
{ 		
	// Fast PWM, Top = ICR, Prescaler = 1
	TCCR1A = BIT(WGM11);
	TCCR1B = BIT(WGM13) | BIT(WGM12) | BIT(CS10);
 
	// Adjust timer period so that we get 1cm of
	// measured distance every overflow.
	//
	// Formula:
	// Timer period = clock rate / speed of sound in centimeters * 2
	// The period is multiplied by 2 because the sound travels
	// forth and back.
	ICR1 = 893;
 
	// Enable overflow interrupt
	TIMSK = BIT(TOIE1);
}
 
//
// Delay
// Time in ~60 of microseconds
//
void delay(unsigned int time)
{
	unsigned long end_time, temporary = 0;
 
	// Interrupt safe timestamp waiting
	cli();
	end_time = timestamp + time;
	sei();
 
	while (temporary < end_time)
	{
		cli();
		temporary = timestamp;
		sei();
		asm volatile ("nop");
	}
}
 
//
// Timer1 overflow interrupt
//
ISR(TIMER1_OVF_vect)
{
	timestamp++;	
}
 
//
// Ultrasonic initialization
//
void ultrasonic_init(void)
{
	// Initial state
	ultrasonic.state  = 0;
	ultrasonic.result = 0;
 
	// Setup pins
	pin_setup_output(US_TRIGGER);
	pin_setup_input(US_ECHO);
 
	// External interrupt on any logical change
	EICRB = BIT(ISC40);
 
	// Enable external interrupt 4
	EIMSK = BIT(INT4);
}
 
//
// Trigger ultrasonic pulse
//
void ultrasonic_trigger(void)
{
	// Cannot trigger when listening
	if (ultrasonic.state > 0)
		return;
 
	// Trigger ultrasonic pulse
	ultrasonic.state = 1;
	pin_set(US_TRIGGER);
	delay(1);
	pin_clear(US_TRIGGER);	
}
 
//
// External interrupt - on echo pulse
//
ISR(INT4_vect)
{
	unsigned char b;	
 
	pin_get_value(US_ECHO, b);
 
	// Rising edge ?
	if (b && (ultrasonic.state == 1))
	{
		ultrasonic.front = timestamp;
		ultrasonic.state = 2;
	}
 
	// Falling edge ?
	else if (!b && (ultrasonic.state == 2))
	{		
		ultrasonic.result = timestamp - ultrasonic.front;
		ultrasonic.state = 0;
	}
}
 
//
// Get ultrasonic result
//
unsigned short ultrasonic_getresult(void)
{
	unsigned short result;
 
	// Interrupt safe result reading
	cli();
	result = ultrasonic.result;	
	sei();
	asm volatile ("nop");
 
	return result;
}
 
//
// Program entrance function
//
int main(void)
{ 		 
	unsigned short value = 0;
	char buffer[8];
 
	// Initializing
	clock_init();
	ultrasonic_init();	
	pin_setup_output(LEDR);
 
	// Init display
	lcd_init(LCD_DISP_ON);
	lcd_gotoxy(0, 0);	
	lcd_puts("Distance: ");
 
	// Enable global interrupts
	sei();	
 
 	// Endless loop
	while (1)
	{		
		// Blink LED constantly
		cli();
		pin_set_to(LEDR, (timestamp % 10000) > 1000);
		sei();
 
		// Trigger ultrasonic pulse
		ultrasonic_trigger();
 
		// Read last ultrasonic result			
		INTEGRATE(value, ultrasonic_getresult(), INTEGRATION_STEP);
 
		// Go-to first row and to first character
		lcd_gotoxy(0, 1);	
 
		// Output text
		itoa(value, buffer, 10);		
		lcd_puts(buffer);
		lcd_puts("   ");
 
		// Delay a while
		delay(200);
	}
}