Ultraheli kaugusmõõdik

Kasutatud on SRF 04 kaugusmõõdikut.

//
// SFR04 ultrasonic distance measuring
//
// Mikk Leini
//
// 2009
//
 
// Include avrlibc
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
 
// Include common library
#include "pinops.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 SEGMENT_DISPLAY_LATCH      PORTPIN(G, 2)
#define SEGMENT_DISPLAY_DATA_OUT   PORTPIN(C, 6)
#define SEGMENT_DISPLAY_CLOCK      PORTPIN(C, 7)
#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;
 
//
// 7 segment display initialization
//
void segment_diplay_init(void)
{
	// Set latch, data out and clock pins as output
	setup_output_pin(SEGMENT_DISPLAY_LATCH);
	setup_output_pin(SEGMENT_DISPLAY_DATA_OUT);
	setup_output_pin(SEGMENT_DISPLAY_CLOCK);	
}
 
//
// Digit writing to 7 segment display
//
void segment_display_write(unsigned char digit)
{
	unsigned char map;
 
	// Decimal to segment map
	switch (digit)
	{
		case 0 : map = 0b00111111; break; // Every bit corresponds to one segment
		case 1 : map = 0b00000110; break; // "1"
		case 2 : map = 0b01011011; break; // "2"
		case 3 : map = 0b01001111; break; // "3" and so on
		case 4 : map = 0b01100110; break; 
		case 5 : map = 0b01101101; break; 
		case 6 : map = 0b01111100; break; 
		case 7 : map = 0b00000111; break;
		case 8 : map = 0b01111111; break; 
		case 9 : map = 0b01100111; break;  
		default: map = 0b01111001;        // E like Error
	}	
 
	// Latch low
	clear_pin(SEGMENT_DISPLAY_LATCH);
 
	// Send every bit in the byte. MSB (most significant bit) first.
	for (signed char i = 7; i >= 0; i--)
	{
		// If bit is set, sets the data out pin, otherwise not		
		set_pin_to(SEGMENT_DISPLAY_DATA_OUT, IS_BIT_SET(map, i));
 
		// Clock high for certain period
		set_pin(SEGMENT_DISPLAY_CLOCK)
		_delay_us(1);
 
		// Clock low for certain period		
		clear_pin(SEGMENT_DISPLAY_CLOCK)
		_delay_us(1);
	}
 
	// Latch high 	
	set_pin(SEGMENT_DISPLAY_LATCH);
}
 
 
//
// 0.1ms clock Timer1
//
void clock_init(void)
{ 		
	// Fast PWM, Top = ICR, Prescaler = 1
	TCCR1A = BIT(WGM11);
	TCCR1B = BIT(WGM13) | BIT(WGM12) | BIT(CS10);
 
	// Top = 1475
	// Clock = 14.745600 Mhz / 1475 = ~10 kHz
	// Period = 1 / Clock = 0.1 ms
	ICR1 = 1475;
 
	// Enable overflow interrupt
	TIMSK = BIT(TOIE1);
}
 
//
// Delay
// Time in 0.1 of milliseconds
//
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
	setup_output_pin(US_TRIGGER);
	setup_input_pin(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;
	set_pin(US_TRIGGER);
	delay(1);
	clear_pin(US_TRIGGER);	
}
 
//
// External interrupt - on echo pulse
//
ISR(INT4_vect)
{
	unsigned char b;	
 
	get_pin_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)
{ 		 
	volatile unsigned short lowValue  = 0xFFFF;
	volatile unsigned short highValue = 0x0000;
	volatile unsigned short value = 0;
	volatile unsigned char relativeValue;
 
	// Initializing
	clock_init();
	ultrasonic_init();
	segment_diplay_init();	
	setup_output_pin(LEDR);
 
	// Enable global interrupts
	sei();	
 
 	// Endless loop
	while (1)
	{		
		// Blink LED every second
		cli();
		set_pin_to(LEDR, (timestamp % 10000) > 1000);							
		sei();
 
		// Constantly try triggering ultrasonic pulses
		ultrasonic_trigger();
 
		// Read ultrasonic result	
		INTEGRATE(value, ultrasonic_getresult(), INTEGRATION_STEP);
 
		// Check for new lower limit
		lowValue = MIN(lowValue, value);
 
		// Check for new higher limit
		highValue = MAX(highValue, value);
 
		// Calculate relative value
		// Range from lowValue to highValue is converted to 0 to 9		
		relativeValue = 9 * (value - lowValue) / (highValue - lowValue);
 
		// Write digit
		segment_display_write(relativeValue);		
 
		// Delay a while
		delay(50);
	}
}